use crate::*;
use crate::util::*;
use wide::f32x4;
use std::ops::*;
macro_rules! rotor2s {
($($rn:ident => ($mt:ident, $vt:ident, $bt:ident, $t:ident)),+) => {
$(
#[derive(Clone, Copy, Debug)]
#[repr(C)]
pub struct $rn {
pub s: $t,
pub bv: $bt,
}
derive_default_identity!($rn);
impl $rn {
#[inline]
pub fn new(scalar: $t, bivector: $bt) -> Self {
Self {
s: scalar,
bv: bivector,
}
}
#[inline]
pub fn identity() -> Self {
Self {
s: $t::from(1.0),
bv: $bt::zero(),
}
}
#[inline]
pub fn from_rotation_between(from: $vt, to: $vt) -> Self {
Self::new(
$t::from(1.0) + to.dot(from),
to.wedge(from)).normalized()
}
#[inline]
pub fn from_angle_plane(planeangle: $bt) -> Self {
let angle = planeangle.mag();
let plane = planeangle / angle;
let half_angle = angle / $t::from(2.0);
let (sin, cos) = half_angle.sin_cos();
Self::new(cos, plane * -sin)
}
#[inline]
pub fn from_angle(angle: $t) -> Self {
let half_angle = angle / $t::from(2.0);
let (sin, cos) = half_angle.sin_cos();
Self::new(cos, $bt::new(-sin))
}
#[inline]
pub fn mag_sq(&self) -> $t {
self.s.mul_add(self.s, self.bv.mag_sq())
}
#[inline]
pub fn mag(&self) -> $t {
self.mag_sq().sqrt()
}
#[inline]
pub fn normalize(&mut self) {
let mag = self.mag();
self.s /= mag;
self.bv.xy /= mag;
}
#[inline]
pub fn normalized(&self) -> Self {
let mut s = *self;
s.normalize();
s
}
#[inline]
pub fn reverse(&mut self) {
self.bv = -self.bv;
}
#[inline]
pub fn reversed(&self) -> Self {
let mut s = *self;
s.reverse();
s
}
#[inline]
pub fn rotate_by(&mut self, other: Self) {
let b = *self;
let a = other;
let sa2_plus_baxy2 = a.s.mul_add(a.s, a.bv.xy * a.bv.xy);
self.s = (a.s - b.s) * a.bv.xy * b.bv.xy
+ b.s * sa2_plus_baxy2;
self.bv.xy = b.bv.xy * sa2_plus_baxy2;
}
#[inline]
pub fn rotated_by(mut self, other: Self) -> Self {
self.rotate_by(other);
self
}
#[inline]
pub fn rotate_vec(self, vec: &mut $vt) {
let s2_minus_bxy2 = self.s * self.s - self.bv.xy * self.bv.xy;
let two_s_bxy = $t::from(2.0) * self.s * self.bv.xy;
let v = *vec;
vec.x = s2_minus_bxy2.mul_add(v.x, two_s_bxy * v.y);
vec.y = s2_minus_bxy2.mul_add(v.y, -(two_s_bxy * v.x));
}
#[inline]
pub fn into_matrix(self) -> $mt {
let s2_minus_bxy2 = self.s * self.s - self.bv.xy * self.bv.xy;
let two_s_bxy = $t::from(2.0) * self.s * self.bv.xy;
$mt::new(
$vt::new(
s2_minus_bxy2,
-two_s_bxy),
$vt::new(
two_s_bxy,
s2_minus_bxy2))
}
#[inline]
pub fn layout() -> alloc::alloc::Layout {
alloc::alloc::Layout::from_size_align(std::mem::size_of::<Self>(), std::mem::align_of::<$t>()).unwrap()
}
}
impl From<$rn> for $mt {
#[inline]
fn from(rotor: $rn) -> $mt {
rotor.into_matrix()
}
}
impl EqualsEps for $rn {
fn eq_eps(self, other: Self) -> bool {
self.s.eq_eps(other.s) && self.bv.eq_eps(other.bv)
}
}
impl Mul for $rn {
type Output = Self;
#[inline]
fn mul(self, rhs: Self) -> Self {
Self {
s: self.s.mul_add(rhs.s, -(self.bv.xy * rhs.bv.xy)),
bv: $bt {
xy: self.s.mul_add(rhs.bv.xy, rhs.s * self.bv.xy)
}
}
}
}
impl AddAssign for $rn {
#[inline]
fn add_assign(&mut self, rhs: Self) {
self.s += rhs.s;
self.bv += rhs.bv;
}
}
impl Add for $rn {
type Output = Self;
#[inline]
fn add(mut self, rhs: Self) -> Self {
self += rhs;
self
}
}
impl SubAssign for $rn {
#[inline]
fn sub_assign(&mut self, rhs: Self) {
self.s -= rhs.s;
self.bv -= rhs.bv;
}
}
impl Sub for $rn {
type Output = Self;
#[inline]
fn sub(mut self, rhs: Self) -> Self {
self -= rhs;
self
}
}
impl Mul<$vt> for $rn {
type Output = $vt;
#[inline]
fn mul(self, mut rhs: $vt) -> $vt {
self.rotate_vec(&mut rhs);
rhs
}
}
impl MulAssign<$t> for $rn {
#[inline]
fn mul_assign(&mut self, rhs: $t) {
self.s /= rhs;
self.bv /= rhs;
}
}
impl Mul<$t> for $rn {
type Output = Self;
#[inline]
fn mul(mut self, rhs: $t) -> Self {
self *= rhs;
self
}
}
impl Mul<$rn> for $t {
type Output = $rn;
#[inline]
fn mul(self, rotor: $rn) -> $rn {
rotor * self
}
}
)+
}
}
rotor2s!(Rotor2 => (Mat2, Vec2, Bivec2, f32), WRotor2 => (Wat2, Wec2, WBivec2, f32x4));
macro_rules! rotor3s {
($($rn:ident => ($mt:ident, $vt:ident, $bt:ident, $t:ident)),+) => {
$(
#[derive(Clone, Copy, Debug)]
#[repr(C)]
pub struct $rn {
pub s: $t,
pub bv: $bt,
}
derive_default_identity!($rn);
impl $rn {
#[inline]
pub fn new(scalar: $t, bivector: $bt) -> Self {
Self {
s: scalar,
bv: bivector,
}
}
#[inline]
pub fn identity() -> Self {
Self {
s: $t::from(1.0),
bv: $bt::zero(),
}
}
#[inline]
pub fn from_rotation_between(from: $vt, to: $vt) -> Self {
Self::new(
$t::from(1.0) + to.dot(from),
to.wedge(from)).normalized()
}
#[inline]
pub fn from_angle_plane(angle: $t, plane: $bt) -> Self {
let half_angle = angle / $t::from(2.0);
let (sin, cos) = half_angle.sin_cos();
Self::new(cos, plane * -sin)
}
#[inline]
pub fn from_rotation_xy(angle: $t) -> Self {
Self::from_angle_plane(angle, $bt::unit_xy())
}
#[inline]
pub fn from_rotation_xz(angle: $t) -> Self {
Self::from_angle_plane(angle, $bt::unit_xz())
}
#[inline]
pub fn from_rotation_yz(angle: $t) -> Self {
Self::from_angle_plane(angle, $bt::unit_yz())
}
#[inline]
pub fn from_euler_angles(roll: $t, pitch: $t, yaw: $t) -> Self {
Self::from_angle_plane(yaw, $bt::unit_xz())
* Self::from_angle_plane(pitch, $bt::unit_yz())
* Self::from_angle_plane(roll, $bt::unit_xy())
}
#[inline]
pub fn mag_sq(&self) -> $t {
self.s.mul_add(self.s, self.bv.mag_sq())
}
#[inline]
pub fn mag(&self) -> $t {
self.mag_sq().sqrt()
}
#[inline]
pub fn normalize(&mut self) {
let mag = self.mag();
self.s /= mag;
self.bv.xy /= mag;
self.bv.xz /= mag;
self.bv.yz /= mag;
}
#[inline]
pub fn normalized(&self) -> Self {
let mut s = *self;
s.normalize();
s
}
#[inline]
pub fn reverse(&mut self) {
self.bv = -self.bv;
}
#[inline]
pub fn reversed(&self) -> Self {
let mut s = *self;
s.reverse();
s
}
#[inline]
pub fn rotate_by(&mut self, rhs: Self) {
let b = *self;
let a = rhs;
let two = $t::from(2.0);
let sa2 = a.s * a.s;
let baxy2 = a.bv.xy * a.bv.xy;
let baxz2 = a.bv.xz * a.bv.xz;
let bayz2 = a.bv.yz * a.bv.yz;
let sa_baxy = a.s * a.bv.xy;
let sa_baxz = a.s * a.bv.xz;
let sa_bayz = a.s * a.bv.yz;
let baxy_baxz = a.bv.xy * a.bv.xz;
let baxy_bayz = a.bv.xy * a.bv.yz;
let baxz_bayz = a.bv.xz * a.bv.yz;
let two_bbxy = two * b.bv.xy;
let two_bbxz = two * b.bv.xz;
let two_bbyz = two * b.bv.yz;
self.s = (sa2 + baxy2 + baxz2 + bayz2) * b.s;
self.bv.xy = (sa2 + baxy2 - baxz2 - bayz2).mul_add(
b.bv.xy,
(baxy_baxz + sa_bayz).mul_add(
two_bbxz,
(baxy_bayz - sa_baxz) * two_bbyz));
self.bv.xz = (sa2 - baxy2 + baxz2 - bayz2).mul_add(
b.bv.xz,
(baxy_baxz - sa_bayz).mul_add(
two_bbxy,
(baxz_bayz + sa_baxy) * two_bbyz));
self.bv.yz = (sa2 - baxy2 - baxz2 + bayz2).mul_add(
b.bv.yz,
(baxy_bayz + sa_baxz).mul_add(
two_bbxy,
(baxz_bayz - sa_baxy) * two_bbxz));
}
#[inline]
pub fn rotated_by(mut self, rhs: Self) -> Self {
self.rotate_by(rhs);
self
}
#[inline]
pub fn rotate_vec(self, vec: &mut $vt) {
let s2 = self.s * self.s;
let bxy2 = self.bv.xy * self.bv.xy;
let bxz2 = self.bv.xz * self.bv.xz;
let byz2 = self.bv.yz * self.bv.yz;
let two = $t::from(2.0);
let s_bxy = self.s * self.bv.xy;
let s_bxz = self.s * self.bv.xz;
let s_byz = self.s * self.bv.yz;
let bxz_byz = self.bv.xz * self.bv.yz;
let bxy_byz = self.bv.xy * self.bv.yz;
let bxy_bxz = self.bv.xy * self.bv.xz;
let two_vx = two * vec.x;
let two_vy = two * vec.y;
let two_vz = two * vec.z;
vec.x = vec.x.mul_add(
s2 - bxy2 - bxz2 + byz2,
two_vy.mul_add(
s_bxy - bxz_byz,
two_vz * (s_bxz + bxy_byz)));
vec.y = two_vx.mul_add(
-(bxz_byz + s_bxy),
vec.y.mul_add(
s2 - bxy2 + bxz2 - byz2,
two_vz * (s_byz - bxy_bxz)));
vec.z = two_vx.mul_add(
bxy_byz - s_bxz,
-two_vy.mul_add(
bxy_bxz + s_byz,
-(vec.z * (s2 + bxy2 - bxz2 - byz2))));
}
#[inline]
pub fn into_matrix(self) -> $mt {
let s2 = self.s * self.s;
let bxy2 = self.bv.xy * self.bv.xy;
let bxz2 = self.bv.xz * self.bv.xz;
let byz2 = self.bv.yz * self.bv.yz;
let s_bxy = self.s * self.bv.xy;
let s_bxz = self.s * self.bv.xz;
let s_byz = self.s * self.bv.yz;
let bxz_byz = self.bv.xz * self.bv.yz;
let bxy_byz = self.bv.xy * self.bv.yz;
let bxy_bxz = self.bv.xy * self.bv.xz;
let two = $t::from(2.0);
$mt::new(
$vt::new(
s2 - bxy2 - bxz2 + byz2,
-two * (bxz_byz + s_bxy),
two * (bxy_byz - s_bxz)),
$vt::new(
two * (s_bxy - bxz_byz),
s2 - bxy2 + bxz2 - byz2,
-two * (s_byz + bxy_bxz)
),
$vt::new(
two * (s_bxz + bxy_byz),
two * (s_byz - bxy_bxz),
s2 + bxy2 - bxz2 - byz2
)
)
}
#[inline]
pub fn layout() -> alloc::alloc::Layout {
alloc::alloc::Layout::from_size_align(std::mem::size_of::<Self>(), std::mem::align_of::<$t>()).unwrap()
}
}
impl From<$rn> for $mt {
#[inline]
fn from(rotor: $rn) -> $mt {
rotor.into_matrix()
}
}
impl EqualsEps for $rn {
#[inline]
fn eq_eps(self, other: Self) -> bool {
self.s.eq_eps(other.s) && self.bv.eq_eps(other.bv)
}
}
impl Mul for $rn {
type Output = Self;
#[inline]
fn mul(self, q: Self) -> Self {
Self {
s: self.s.mul_add(q.s, -self.bv.xy.mul_add(q.bv.xy, self.bv.xz.mul_add(q.bv.xz, self.bv.yz * q.bv.yz))),
bv: $bt {
xy: self.bv.xy.mul_add(q.s, self.s.mul_add(q.bv.xy, self.bv.yz.mul_add(q.bv.xz, -(self.bv.xz * q.bv.yz)))),
xz: self.bv.xz.mul_add(q.s, self.s.mul_add(q.bv.xz, -self.bv.yz.mul_add(q.bv.xy, -(self.bv.xy * q.bv.yz)))),
yz: self.bv.yz.mul_add(q.s, self.s.mul_add(q.bv.yz, self.bv.xz.mul_add(q.bv.xy, -(self.bv.xy * q.bv.xz)))),
}
}
}
}
impl AddAssign for $rn {
#[inline]
fn add_assign(&mut self, rhs: Self) {
self.s += rhs.s;
self.bv += rhs.bv;
}
}
impl Add for $rn {
type Output = Self;
#[inline]
fn add(mut self, rhs: Self) -> Self {
self += rhs;
self
}
}
impl SubAssign for $rn {
#[inline]
fn sub_assign(&mut self, rhs: Self) {
self.s -= rhs.s;
self.bv -= rhs.bv;
}
}
impl Sub for $rn {
type Output = Self;
#[inline]
fn sub(mut self, rhs: Self) -> Self {
self -= rhs;
self
}
}
impl Mul<$vt> for $rn {
type Output = $vt;
#[inline]
fn mul(self, mut rhs: $vt) -> $vt {
self.rotate_vec(&mut rhs);
rhs
}
}
impl MulAssign<$t> for $rn {
#[inline]
fn mul_assign(&mut self, rhs: $t) {
self.s /= rhs;
self.bv /= rhs;
}
}
impl Mul<$t> for $rn {
type Output = Self;
#[inline]
fn mul(mut self, rhs: $t) -> Self {
self *= rhs;
self
}
}
impl Mul<$rn> for $t {
type Output = $rn;
#[inline]
fn mul(self, rotor: $rn) -> $rn {
rotor * self
}
}
)+
}
}
rotor3s!(Rotor3 => (Mat3, Vec3, Bivec3, f32), WRotor3 => (Wat3, Wec3, WBivec3, f32x4));
#[cfg(test)]
mod test {
use super::*;
#[test]
pub fn rotate_vector_roundtrip() {
let a = Vec3::new(1.0, 2.0, -5.0).normalized();
let b = Vec3::new(1.0, 1.0, 1.0).normalized();
let c = Vec3::new(2.0, 3.0, -3.0).normalized();
let rotor_ab = Rotor3::from_rotation_between(a, b);
let rotor_bc = Rotor3::from_rotation_between(b, c);
let rot_ab = rotor_ab * a;
let rot_bc = rotor_bc * b;
let rot_abc = rotor_bc * (rotor_ab * a);
println!("{:?} = {:?}", rot_ab, b);
println!("{:?} = {:?}", rot_bc, c);
println!("{:?} = {:?}", rot_abc, c);
assert!(rot_ab.eq_eps(b));
assert!(rot_bc.eq_eps(c));
assert!(rot_abc.eq_eps(c));
}
#[test]
pub fn rotate_rotor_trivial() {
let a = Vec3::new(1.0, 2.0, -5.0).normalized();
let b = Vec3::new(1.0, 1.0, 1.0).normalized();
let c = Vec3::new(2.0, 3.0, -3.0).normalized();
let r_ab = Rotor3::from_rotation_between(a, b);
let r_bc = Rotor3::from_rotation_between(b, c);
let res = r_ab.rotated_by(r_bc).rotated_by(r_bc.reversed());
println!("{:?} {:?}", r_ab, res);
assert!(r_ab.eq_eps(res));
}
#[test]
pub fn compose_rotor_roundtrip() {
let a = Vec3::new(0.25, -5.0, 1.0).normalized();
let b = Vec3::new(-5.0, 2.0, 4.0).normalized();
let c = Vec3::new(-3.0, 0.0, -1.0).normalized();
let rotor_ab = Rotor3::from_rotation_between(a, b);
let rotor_bc = Rotor3::from_rotation_between(b, c);
let rotor_abbc = rotor_bc * rotor_ab;
let res = rotor_abbc * a;
println!("{:#?} {:#?}", rotor_abbc, res);
assert!(c.eq_eps(res));
}
}