use crate::util::*;
use crate::*;
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 const fn new(scalar: $t, bivector: $bt) -> Self {
Self {
s: scalar,
bv: bivector,
}
}
#[inline]
pub fn identity() -> Self {
Self {
s: $t::splat(1.0),
bv: $bt::zero(),
}
}
#[inline]
pub fn from_rotation_between(from: $vt, to: $vt) -> Self {
Self::new(
$t::splat(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::splat(0.5);
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::splat(2.0);
let (sin, cos) = half_angle.sin_cos();
Self::new(cos, $bt::new(-sin))
}
#[inline]
pub fn mag_sq(&self) -> $t {
self.s * 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 dot(&self, rhs: Self) -> $t {
self.s * rhs.s + self.bv.dot(rhs.bv)
}
#[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 fx = self.s * vec.x + self.bv.xy * vec.y;
let fy = self.s * vec.y - (self.bv.xy * vec.x);
vec.x = self.s * fx - (self.bv.xy * fy);
vec.y = self.s * fy + self.bv.xy * fx;
}
#[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::splat(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 * rhs.s - (self.bv.xy * rhs.bv.xy),
bv: $bt {
xy: self.s * 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
}
}
impl DivAssign<$t> for $rn {
#[inline]
fn div_assign(&mut self, rhs: $t) {
self.s /= rhs;
self.bv /= rhs;
}
}
impl Div<$t> for $rn {
type Output = Self;
#[inline]
fn div(mut self, rhs: $t) -> Self {
self /= rhs;
self
}
}
)+
}
}
rotor2s!(
Rotor2 => (Mat2, Vec2, Bivec2, f32),
Rotor2x4 => (Mat2x4, Vec2x4, Bivec2x4, f32x4),
Rotor2x8 => (Mat2x8, Vec2x8, Bivec2x8, f32x8)
);
impl std::cmp::PartialEq for Rotor2 {
fn eq(&self, other: &Self) -> bool {
self.s == other.s && self.bv == other.bv
}
}
#[cfg(feature = "f64")]
rotor2s!(
DRotor2 => (DMat2, DVec2, DBivec2, f64),
DRotor2x2 => (DMat2x2, DVec2x2, DBivec2x2, f64x2),
DRotor2x4 => (DMat2x4, DVec2x4, DBivec2x4, f64x4)
);
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 const fn new(scalar: $t, bivector: $bt) -> Self {
Self {
s: scalar,
bv: bivector,
}
}
#[inline]
pub fn identity() -> Self {
Self {
s: $t::splat(1.0),
bv: $bt::zero(),
}
}
#[inline]
pub fn from_rotation_between(from: $vt, to: $vt) -> Self {
Self::new(
$t::splat(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::splat(0.5);
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 * 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 dot(&self, rhs: Self) -> $t {
self.s * rhs.s + self.bv.dot(rhs.bv)
}
#[inline]
pub fn rotate_by(&mut self, rhs: Self) {
let b = *self;
let a = rhs;
let two = $t::splat(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) * b.bv.xy
+ (baxy_baxz + sa_bayz) * two_bbxz
+ (baxy_bayz - sa_baxz) * two_bbyz;
self.bv.xz = (sa2 - baxy2 + baxz2 - bayz2) * b.bv.xz
+ (baxy_baxz - sa_bayz) * two_bbxy
+ (baxz_bayz + sa_baxy) * two_bbyz;
self.bv.yz = (sa2 - baxy2 - baxz2 + bayz2) * b.bv.yz
+ (baxy_bayz + sa_baxz) * 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 fx = self.s * vec.x + self.bv.xy * vec.y + self.bv.xz * vec.z;
let fy = self.s * vec.y - self.bv.xy * vec.x + self.bv.yz * vec.z;
let fz = self.s * vec.z - self.bv.xz * vec.x - self.bv.yz * vec.y;
let fw = self.bv.xy * vec.z - self.bv.xz * vec.y + self.bv.yz * vec.x;
vec.x = self.s * fx + self.bv.xy * fy + self.bv.xz * fz + self.bv.yz * fw;
vec.y = self.s * fy - self.bv.xy * fx - self.bv.xz * fw + self.bv.yz * fz;
vec.z = self.s * fz + self.bv.xy * fw - self.bv.xz * fx - self.bv.yz * fy;
}
pub fn rotate_vecs(self, vecs: &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 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 xa = s2 - bxy2 - bxz2 + byz2;
let xb = s_bxy - bxz_byz;
let xc = s_bxz + bxy_byz;
let ya = -(bxz_byz + s_bxy);
let yb = s2 - bxy2 + bxz2 - byz2;
let yc = s_byz - bxy_bxz;
let za = bxy_byz - s_bxz;
let zb = bxy_bxz + s_byz;
let zc = -(s2 + bxy2 - bxz2 - byz2);
for vec in vecs {
let two_vx = vec.x + vec.x;
let two_vy = vec.y + vec.y;
let two_vz = vec.z + vec.z;
vec.x = vec.x * xa + two_vy * xb + two_vz * xc;
vec.y = two_vx * ya + vec.y * yb + two_vz * yc;
vec.z = two_vx * za - two_vy * zb - vec.z * zc;
}
}
#[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::splat(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 * q.s - self.bv.xy * q.bv.xy - self.bv.xz * q.bv.xz - self.bv.yz * q.bv.yz,
bv: $bt {
xy: self.bv.xy * q.s + self.s * q.bv.xy + self.bv.yz * q.bv.xz - self.bv.xz * q.bv.yz,
xz: self.bv.xz * q.s + self.s * q.bv.xz - self.bv.yz * q.bv.xy + self.bv.xy * q.bv.yz,
yz: self.bv.yz * q.s + self.s * q.bv.yz + self.bv.xz * 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
}
}
impl DivAssign<$t> for $rn {
#[inline]
fn div_assign(&mut self, rhs: $t) {
self.s /= rhs;
self.bv /= rhs;
}
}
impl Div<$t> for $rn {
type Output = Self;
#[inline]
fn div(mut self, rhs: $t) -> Self {
self /= rhs;
self
}
}
)+
}
}
rotor3s!(
Rotor3 => (Mat3, Vec3, Bivec3, f32),
Rotor3x4 => (Mat3x4, Vec3x4, Bivec3x4, f32x4),
Rotor3x8 => (Mat3x8, Vec3x8, Bivec3x8, f32x8)
);
impl std::cmp::PartialEq for Rotor3 {
fn eq(&self, other: &Self) -> bool {
self.s == other.s && self.bv == other.bv
}
}
#[cfg(feature = "f64")]
rotor3s!(
DRotor3 => (DMat3, DVec3, DBivec3, f64),
DRotor3x2 => (DMat3x2, DVec3x2, DBivec3x2, f64x2),
DRotor3x4 => (DMat3x4, DVec3x4, DBivec3x4, f64x4)
);
#[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));
}
#[test]
pub fn rotor_interp_trivial() {
let i = Rotor3::identity();
let interp = i.lerp(i, 0.5);
println!("{:#?} ::: {:#?}", i, interp);
assert!(interp.eq_eps(i))
}
}