Compiler Explorer

Source code

#![feature(adt_const_params, generic_const_exprs, core_intrinsics)]

use std::marker::ConstParamTy;
use std::ops::{Mul, BitAnd, BitXor};

#[derive(Copy, Clone)]
pub struct MVec<const BASIS: &'static [Basis]>
where [(); BASIS.len()]: {
    components: [f32; BASIS.len()],
}

#[derive(ConstParamTy, PartialEq, Eq, Copy, Clone)]
pub struct Basis {
    p: u32,
    n: u32,
    z: u32,
}

enum Sign {
    Zero,
    Pos,
    Neg,
}

const S : Basis = Basis{p: 0, n: 0, z: 0};
pub type Scalar = MVec<{&[S]}>;
const E0 : Basis = Basis{p: 0, n: 0, z: 1};
const E1 : Basis = Basis{p: 0b0010, n: 0, z: 0};
const E2 : Basis = Basis{p: 0b0100, n: 0, z: 0};
const E3 : Basis = Basis{p: 0b1000, n: 0, z: 0};
const E12 : Basis = Basis{p: 0b0110, n: 0, z: 0};
const E13 : Basis = Basis{p: 0b1010, n: 0, z: 0};
const E23 : Basis = Basis{p: 0b1100, n: 0, z: 0};
const E01 : Basis = Basis{p: 0b0010, n: 0, z: 1};
const E02 : Basis = Basis{p: 0b0100, n: 0, z: 1};
const E03 : Basis = Basis{p: 0b1000, n: 0, z: 1};
const E012 : Basis = Basis{p: 0b0110, n: 0, z: 1};
const E013 : Basis = Basis{p: 0b1010, n: 0, z: 1};
const E023 : Basis = Basis{p: 0b1100, n: 0, z: 1};
const E123 : Basis = Basis{p: 0b1110, n: 0, z: 0};
const E0123 : Basis = Basis{p: 0b1110, n: 0, z: 1};

const PGA3_BASIS: &'static [Basis] = &[ S, E0, E1, E2, E3, E12, E13, E23, E01, E02, E03, E012, E013, E023, E123, E0123 ];

impl Basis {
    #[inline(always)]
    const fn flips(self: Basis, other: Basis) -> Sign {
        let a = self.as_u128();
        let b = other.as_u128();

#[inline(always)]
        const fn compute_swaps(a: u128, b: u128) -> usize {
            if a == 0 {
                0
            } else {
                (a & b).count_ones() as usize + compute_swaps(a >> 1, b)
            }
        }
        let swaps = compute_swaps(a >> 1, b);
        if (swaps + (self.n & other.n).count_ones() as usize) % 2 == 0 {
            Sign::Pos
        } else {
            Sign::Neg
        }
    }
    #[inline(always)]
    const fn flips_mul(self: Basis, other: Basis) -> Sign {
        if self.z & other.z != 0 {
            return Sign::Zero;
        } else {
            self.flips(other)
        }
    }
    #[inline(always)]
    const fn flips_outer(self: Basis, other: Basis) -> Sign {
        if self.p & other.p != 0 || self.n & other.n != 0 || self.z & other.z != 0 {
            return Sign::Zero;
        } else {
            self.flips(other)
        }
    }
    #[inline(always)]
    fn flips_inner(self: Basis, other: Basis) -> Sign {
        let pss = self.as_u128() | other.as_u128();
        if (pss == other.as_u128() || pss == self.as_u128()) && (self.z & other.z == 0) {
            self.flips(other)
        } else {
            Sign::Zero
        }
    }
    #[inline(always)]
    const fn as_u128(self) -> u128 {
        (self.p as u128) | (self.n as u128) << 32 | (self.z as u128) << 64
    }
    #[inline(always)]
    const fn mul(self, other: Self) -> Self {
        Self {
            p: self.p ^ other.p,
            n: self.n ^ other.n,
            z: self.z ^ other.z,
        }
    }
    #[inline(always)]
    const fn or(self, other: Self) -> Self {
        Self {
            p: self.p | other.p,
            n: self.n | other.n,
            z: self.z | other.z,
        }
    }
}

impl<const BASIS: &'static [Basis]> MVec<BASIS>
where [(); BASIS.len()]:
{
    pub fn reverse(mut self) -> Self {
        for (b, x) in BASIS.iter().zip(self.components.iter_mut()) {
            if let Some(total) = b.as_u128().count_ones().checked_sub(1) {
                // Triangular number formula
                if ((total + 1) * total) / 2 % 2 != 0 {
                    *x = -*x
                }
            }
        }
        self
    }

const fn pss<const A_BASIS: &'static [Basis]>() -> Basis {
        let mut acc = S;
        let mut i = 0;
        while i < BASIS.len() {
            acc = acc.or(BASIS[i]);
            i += 1;
        }
        let mut i = 0;
        while i < A_BASIS.len() {
            acc = acc.or(A_BASIS[i]);
            i += 1;
        }
        acc
    }

pub fn dual<const OUT_BASIS: &'static [Basis]>(self) -> MVec<OUT_BASIS>
    where [(); OUT_BASIS.len()]:
    {
        // Workaround for inability to generate the OUT_BASIS at compile-time
        assert_eq!(BASIS.len(), OUT_BASIS.len());

let pss = Self::pss::<OUT_BASIS>();

let mut out = [0.0; OUT_BASIS.len()];
        for (&x_b, x) in BASIS.iter().zip(self.components) {
            let out_b = x_b.mul(pss);
            let (_, cell) = OUT_BASIS.iter().zip(out.iter_mut()).find(|&(&b, _)| b == out_b).unwrap();
            *cell = match x_b.flips(pss) {
                Sign::Neg => -x,
                Sign::Pos => x,
                Sign::Zero => unreachable!(),
            };
        }
        MVec { components: out }
    }

pub fn undual<const PSS: Basis, const OUT_BASIS: &'static [Basis]>(self) -> MVec<OUT_BASIS>
    where [(); OUT_BASIS.len()]:
    {
        // Workaround for inability to generate the OUT_BASIS at compile-time
        assert_eq!(BASIS.len(), OUT_BASIS.len());

let pss = Self::pss::<OUT_BASIS>();

let mut out = [0.0; OUT_BASIS.len()];
        for (&x_b, x) in BASIS.iter().zip(self.components) {
            let out_b = x_b.mul(pss);
            let (_, cell) = OUT_BASIS.iter().zip(out.iter_mut()).find(|&(&b, _)| b == out_b).unwrap();
            *cell = match pss.flips(x_b) {
                Sign::Neg => -x,
                Sign::Pos => x,
                Sign::Zero => unreachable!(),
            };
        }
        MVec { components: out }
    }

//#[inline(always)]
    pub fn from_prod<const X_BASIS: &'static [Basis], const Y_BASIS: &'static [Basis]>(x: MVec<X_BASIS>, y: MVec<Y_BASIS>) -> Self
    where
        [(); X_BASIS.len()]:,
        [(); Y_BASIS.len()]:,
    {

use std::intrinsics::{fadd_fast, fsub_fast, fmul_fast};

let mut out = [0.0; BASIS.len()];
        for (&x_b, x) in X_BASIS.iter().zip(x.components) {
            for (&y_b, y) in Y_BASIS.iter().zip(y.components) {
                let out_b = x_b.mul(y_b);
                if let Some((_, cell)) = BASIS.iter().zip(out.iter_mut()).find(|&(&b, _)| b == out_b) {
                    match x_b.flips_mul(y_b) {
                        Sign::Pos => *cell = unsafe { fadd_fast(*cell, fmul_fast(x, y)) },
                        Sign::Neg => *cell = unsafe { fsub_fast(*cell, fmul_fast(x, y)) },
                        Sign::Zero => (),
                    }
                }
            }
        }
        
        Self { components: out }
    }

pub fn from_outer_prod<const X_BASIS: &'static [Basis], const Y_BASIS: &'static [Basis]>(x: MVec<X_BASIS>, y: MVec<Y_BASIS>) -> Self
    where
        [(); X_BASIS.len()]:,
        [(); Y_BASIS.len()]:,
    {

use std::intrinsics::{fadd_fast, fsub_fast, fmul_fast};

let mut out = [0.0; BASIS.len()];
        for (&x_b, x) in X_BASIS.iter().zip(x.components) {
            for (&y_b, y) in Y_BASIS.iter().zip(y.components) {
                let out_b = x_b.mul(y_b);
                if let Some((_, cell)) = BASIS.iter().zip(out.iter_mut()).find(|&(&b, _)| b == out_b) {
                    match x_b.flips_outer(y_b) {
                        Sign::Pos => *cell = unsafe { fadd_fast(*cell, fmul_fast(x, y)) },
                        Sign::Neg => *cell = unsafe { fsub_fast(*cell, fmul_fast(x, y)) },
                        Sign::Zero => (),
                    }
                }
            }
        }
        
        Self { components: out }
    }

pub fn from_inner_prod<const X_BASIS: &'static [Basis], const Y_BASIS: &'static [Basis]>(x: MVec<X_BASIS>, y: MVec<Y_BASIS>) -> Self
    where
        [(); X_BASIS.len()]:,
        [(); Y_BASIS.len()]:,
    {

use std::intrinsics::{fadd_fast, fsub_fast, fmul_fast};

let mut out = [0.0; BASIS.len()];
        for (&x_b, x) in X_BASIS.iter().zip(x.components) {
            for (&y_b, y) in Y_BASIS.iter().zip(y.components) {
                let out_b = x_b.mul(y_b);
                if let Some((_, cell)) = BASIS.iter().zip(out.iter_mut()).find(|&(&b, _)| b == out_b) {
                    match x_b.flips_inner(y_b) {
                        Sign::Pos => *cell = unsafe { fadd_fast(*cell, fmul_fast(x, y)) },
                        Sign::Neg => *cell = unsafe { fsub_fast(*cell, fmul_fast(x, y)) },
                        Sign::Zero => (),
                    }
                }
            }
        }
        
        Self { components: out }
    }
}

macro_rules! impl_mul{
    ($x:ty, $y:ty = $out:ty) => {
        impl Mul<$y> for $x {
            type Output = $out;
            #[inline]
            fn mul(self, rhs: $y) -> Self::Output {
                MVec::from_prod(self, rhs)
            }
        }
    }
}

macro_rules! impl_outer{
    ($x:ty, $y:ty = $out:ty) => {
        impl BitAnd<$y> for $x {
            type Output = $out;
            fn bitand(self, rhs: $y) -> Self::Output {
                MVec::from_outer_prod(self, rhs)
            }
        }
    }
}

macro_rules! impl_inner{
    ($x:ty, $y:ty = $out:ty) => {
        impl BitXor<$y> for $x {
            type Output = $out;
            fn bitxor(self, rhs: $y) -> Self::Output {
                MVec::from_inner_prod(self, rhs)
            }
        }
    }
}

impl_mul!(MVec<{ &[E1, E2] }>, MVec<{ &[E1, E2] }> = MVec<{ &[S, E12] }>);
impl_mul!(MVec<{ &[E1, E2, E3] }>, MVec<{ &[E1, E2, E3] }> = MVec<{ &[S, E12, E13, E23] }>);
impl_outer!(MVec<{ &[E1, E2] }>, MVec<{ &[E1, E2] }> = MVec<{ &[E12] }>);
impl_outer!(MVec<{ &[E1, E2, E3] }>, MVec<{ &[E1, E2, E3] }> = MVec<{ &[E12, E13, E23] }>);
impl_inner!(MVec<{ &[E1, E2, E3] }>, MVec<{ &[E12, E13, E23] }> = MVec<{ &[E1, E2, E3] }>);
impl_inner!(MVec<{ &[E1, E2, E3] }>, MVec<{ &[E1, E2, E3] }> = MVec<{ &[S] }>);

pub type Motor = MVec<{ &[S, E12, E13, E23, E01, E02, E03, E0123] }>;
pub type Point = MVec<{ &[E123, E012, E013, E023] }>;
pub type Flector = MVec<{ &[E1, E2, E3, E0, E123, E012, E013, E023] }>;

impl_mul!(Motor, Point = Flector);
impl_mul!(Flector, Motor = Flector);
pub extern "Rust" fn mulc(m: Motor, p: Point) -> Point {
    Point::from_prod(m * p * m.reverse(), Scalar{ components: [1.0]})
}