lav/example/mod.rs
1// Copyright © 2021-2025 Rouven Spreckels <rs@qu1x.dev>
2//
3// This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of
4// the MPL was not distributed with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
5
6//! Portably SIMD-optimized 3D rotator implementation generic over lane type [`f32`] and [`f64`].
7//!
8//! ```
9//! #![allow(non_snake_case)]
10//! #![feature(portable_simd)]
11//!
12//! use core::{
13//! mem::transmute,
14//! ops::{
15//! Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Shl, ShlAssign, Sub, SubAssign,
16//! },
17//! };
18//! use lav::{ApproxEq, Real, SimdMask, SimdReal, swizzle};
19//!
20//! #[derive(Debug, Copy, Clone, PartialEq, Eq)]
21//! #[repr(transparent)]
22//! pub struct Rotator3<R: Real> {
23//! wxyz: R::Simd<4>,
24//! }
25//!
26//! impl<R: Real> Rotator3<R> {
27//! pub fn new(alpha: R, x: R, y: R, z: R) -> Self {
28//! let (sin, cos) = (alpha * -R::FRAC_1_2).sin_cos();
29//! (Self::from([R::ZERO, x, y, z]).unit() * sin).set_w(cos)
30//! }
31//! pub fn from_wxyz(wxyz: [R; 4]) -> Self {
32//! Self { wxyz: wxyz.into() }
33//! }
34//! pub fn from_xyzw(xyzw: [R; 4]) -> Self {
35//! Self {
36//! wxyz: R::Simd::from_array(xyzw).simd_rotate_right::<1>(),
37//! }
38//! }
39//! pub fn norm(&self) -> R {
40//! self.norm_squared().sqrt()
41//! }
42//! pub fn norm_squared(&self) -> R {
43//! (self.wxyz * self.wxyz).reduce_sum()
44//! }
45//! pub fn unit(self) -> Self {
46//! self / self.norm()
47//! }
48//! pub fn inv(self) -> Self {
49//! self.rev() / self.norm_squared()
50//! }
51//! pub fn rev(self) -> Self {
52//! let tfff = R::Simd::mask_flag(0, true);
53//! Self {
54//! wxyz: tfff.select(self.wxyz, -self.wxyz),
55//! }
56//! }
57//! pub fn constrain(self) -> Self {
58//! if self.w().is_sign_negative() {
59//! -self
60//! } else {
61//! self
62//! }
63//! }
64//! pub fn to_wxyz(self) -> [R; 4] {
65//! self.wxyz.to_array()
66//! }
67//! pub fn to_xyzw(self) -> [R; 4] {
68//! self.wxyz.simd_rotate_left::<1>().to_array()
69//! }
70//! pub fn w(&self) -> R {
71//! self.wxyz[0]
72//! }
73//! pub fn x(&self) -> R {
74//! self.wxyz[1]
75//! }
76//! pub fn y(&self) -> R {
77//! self.wxyz[2]
78//! }
79//! pub fn z(&self) -> R {
80//! self.wxyz[3]
81//! }
82//! pub fn set_w(mut self, w: R) -> Self {
83//! self.wxyz[0] = w;
84//! self
85//! }
86//! pub fn set_x(mut self, x: R) -> Self {
87//! self.wxyz[1] = x;
88//! self
89//! }
90//! pub fn set_y(mut self, y: R) -> Self {
91//! self.wxyz[2] = y;
92//! self
93//! }
94//! pub fn set_z(mut self, z: R) -> Self {
95//! self.wxyz[3] = z;
96//! self
97//! }
98//! pub fn point_fn(self) -> impl Fn(&mut Point3<R>) {
99//! // +(+[+1ww+1zz+(+1xx+1yy)]~w+[+0zy+0wx]~w+[+0yw-0zx]~w)e123
100//! // +(+[+1xx+1ww-(+1yy+1zz)]~X+[+2wz+2xy]~Y+[+2zx-2wy]~Z)e032
101//! // +(+[+1yy+1ww-(+1zz+1xx)]~Y+[+2wx+2yz]~Z+[+2xy-2wz]~X)e013
102//! // +(+[+1zz+1ww-(+1xx+1yy)]~Z+[+2wy+2zx]~X+[+2yz-2wx]~Y)e021
103//! let wxyz = self.wxyz;
104//! let zwww = swizzle!(wxyz, [3, 0, 0, 0]);
105//! let xyzx = swizzle!(wxyz, [1, 2, 3, 1]);
106//! let yzxy = swizzle!(wxyz, [2, 3, 1, 2]);
107//! let zttt = R::Simd::from_array([R::ZERO, R::TWO, R::TWO, R::TWO]);
108//! let fttt = R::Simd::mask_flag(0, false);
109//! let pin0 = xyzx.mul_add(xyzx, yzxy * yzxy);
110//! let pin0 = zwww.mul_add(zwww, fttt.negate(pin0));
111//! let pin0 = wxyz.mul_add(wxyz, pin0);
112//! let pin1 = zwww.mul_add(yzxy, wxyz * xyzx) * zttt;
113//! let pin2 = yzxy.mul_add(wxyz, -(zwww * xyzx)) * zttt;
114//! move |point3| {
115//! let wXYZ = point3.wXYZ;
116//! let wYZX = swizzle!(wXYZ, [0, 2, 3, 1]);
117//! let wZXY = swizzle!(wXYZ, [0, 3, 1, 2]);
118//! let wXYZ = pin0.mul_add(wXYZ, pin1.mul_add(wYZX, pin2 * wZXY));
119//! point3.wXYZ = wXYZ;
120//! }
121//! }
122//! pub fn points_fn(self) -> impl Fn(&mut [Point3<R>]) {
123//! let rotate = self.point_fn();
124//! move |points| {
125//! for point3 in points {
126//! rotate(point3);
127//! }
128//! }
129//! }
130//! }
131//!
132//! impl<R: Real> Default for Rotator3<R> {
133//! fn default() -> Self {
134//! Self::from([R::ONE, R::ZERO, R::ZERO, R::ZERO])
135//! }
136//! }
137//!
138//! impl<R: Real> From<[R; 4]> for Rotator3<R> {
139//! fn from(wxyz: [R; 4]) -> Self {
140//! Self::from_wxyz(wxyz)
141//! }
142//! }
143//!
144//! impl<R: Real> Into<[R; 4]> for Rotator3<R> {
145//! fn into(self) -> [R; 4] {
146//! self.to_wxyz()
147//! }
148//! }
149//!
150//! impl<R: Real> ApproxEq<R> for Rotator3<R> {
151//! fn approx_eq(&self, other: &Self, epsilon: R, ulp: R::Bits) -> bool {
152//! self.wxyz.approx_eq(&other.wxyz, epsilon, ulp)
153//! }
154//! }
155//!
156//! impl<R: Real> Add for Rotator3<R> {
157//! type Output = Self;
158//!
159//! fn add(self, other: Self) -> Self::Output {
160//! Self {
161//! wxyz: self.wxyz + other.wxyz,
162//! }
163//! }
164//! }
165//!
166//! impl<R: Real> AddAssign for Rotator3<R> {
167//! fn add_assign(&mut self, other: Self) {
168//! *self = *self + other;
169//! }
170//! }
171//!
172//! impl<R: Real> Div<R> for Rotator3<R> {
173//! type Output = Self;
174//!
175//! fn div(self, other: R) -> Self::Output {
176//! Self {
177//! wxyz: self.wxyz / other.splat(),
178//! }
179//! }
180//! }
181//!
182//! impl<R: Real> DivAssign<R> for Rotator3<R> {
183//! fn div_assign(&mut self, other: R) {
184//! *self = *self / other;
185//! }
186//! }
187//!
188//! impl<R: Real> Mul for Rotator3<R> {
189//! type Output = Self;
190//!
191//! fn mul(self, other: Self) -> Self::Output {
192//! // +(+1LwRw-1LxRx-(+1LyRy+1LzRz))e
193//! // +(+1LwRx-1LyRz+(+1LxRw+1LzRy))e23
194//! // +(+1LwRy-1LzRx+(+1LxRz+1LyRw))e31
195//! // +(+1LwRz-1LxRy+(+1LyRx+1LzRw))e12
196//! let tfff = R::Simd::mask_flag(0, true);
197//! let wxyz = swizzle!(self.wxyz, [0, 0, 0, 0]).mul_add(
198//! other.wxyz,
199//! swizzle!(self.wxyz, [1, 2, 3, 1]).mul_add(
200//! -swizzle!(other.wxyz, [1, 3, 1, 2]),
201//! tfff.negate(swizzle!(self.wxyz, [2, 1, 1, 2]).mul_add(
202//! swizzle!(other.wxyz, [2, 0, 3, 1]),
203//! swizzle!(self.wxyz, [3, 3, 2, 3]) * swizzle!(other.wxyz, [3, 2, 0, 0]),
204//! )),
205//! ),
206//! );
207//! Self { wxyz }
208//! }
209//! }
210//!
211//! impl<R: Real> Mul<R> for Rotator3<R> {
212//! type Output = Self;
213//!
214//! fn mul(self, other: R) -> Self::Output {
215//! Self {
216//! wxyz: self.wxyz * other.splat(),
217//! }
218//! }
219//! }
220//!
221//! impl<R: Real> MulAssign<R> for Rotator3<R> {
222//! fn mul_assign(&mut self, other: R) {
223//! *self = *self * other;
224//! }
225//! }
226//!
227//! impl<R: Real> Neg for Rotator3<R> {
228//! type Output = Self;
229//!
230//! fn neg(self) -> Self::Output {
231//! Self { wxyz: -self.wxyz }
232//! }
233//! }
234//!
235//! impl<R: Real> Sub for Rotator3<R> {
236//! type Output = Self;
237//!
238//! fn sub(self, other: Self) -> Self::Output {
239//! Self {
240//! wxyz: self.wxyz - other.wxyz,
241//! }
242//! }
243//! }
244//!
245//! impl<R: Real> SubAssign for Rotator3<R> {
246//! fn sub_assign(&mut self, other: Self) {
247//! *self = *self - other;
248//! }
249//! }
250//!
251//! #[derive(Debug, Copy, Clone, PartialEq, Eq)]
252//! #[repr(transparent)]
253//! pub struct Point3<R: Real> {
254//! wXYZ: R::Simd<4>,
255//! }
256//!
257//! impl<R: Real> Point3<R> {
258//! pub fn new(w: R, X: R, Y: R, Z: R) -> Self {
259//! Self::from([w, X, Y, Z])
260//! }
261//! pub fn from_wXYZ(wXYZ: [R; 4]) -> Self {
262//! Self { wXYZ: wXYZ.into() }
263//! }
264//! pub fn from_XYZw(XYZw: [R; 4]) -> Self {
265//! Self {
266//! wXYZ: R::Simd::from_array(XYZw).simd_rotate_right::<1>(),
267//! }
268//! }
269//! pub fn as_points(points: &[R]) -> &[Self] {
270//! let (prefix, points, suffix) = R::as_simd::<4>(points);
271//! assert!(prefix.is_empty() && suffix.is_empty(), "misaligned");
272//! // Safe due to `#[repr(transparent)]`.
273//! unsafe { transmute::<&[R::Simd<4>], &[Point3<R>]>(points) }
274//! }
275//! pub fn as_points_mut(points: &mut [R]) -> &mut [Self] {
276//! let (prefix, points, suffix) = R::as_simd_mut::<4>(points);
277//! assert!(prefix.is_empty() && suffix.is_empty(), "misaligned");
278//! // Safe due to `#[repr(transparent)]`.
279//! unsafe { transmute::<&mut [R::Simd<4>], &mut [Point3<R>]>(points) }
280//! }
281//! pub fn norm(&self) -> R {
282//! self.w().abs()
283//! }
284//! pub fn norm_squared(&self) -> R {
285//! let w = self.w();
286//! w * w
287//! }
288//! pub fn unit(self) -> Self {
289//! self / self.norm()
290//! }
291//! pub fn inv(self) -> Self {
292//! self.rev() / self.norm_squared()
293//! }
294//! pub fn rev(self) -> Self {
295//! -self
296//! }
297//! pub fn to_wXYZ(self) -> [R; 4] {
298//! self.wXYZ.to_array()
299//! }
300//! pub fn to_XYZw(self) -> [R; 4] {
301//! self.wXYZ.simd_rotate_left::<1>().to_array()
302//! }
303//! pub fn w(&self) -> R {
304//! self.wXYZ[0]
305//! }
306//! pub fn X(&self) -> R {
307//! self.wXYZ[1]
308//! }
309//! pub fn Y(&self) -> R {
310//! self.wXYZ[2]
311//! }
312//! pub fn Z(&self) -> R {
313//! self.wXYZ[3]
314//! }
315//! pub fn set_w(mut self, w: R) -> Self {
316//! self.wXYZ[0] = w;
317//! self
318//! }
319//! pub fn set_X(mut self, X: R) -> Self {
320//! self.wXYZ[1] = X;
321//! self
322//! }
323//! pub fn set_Y(mut self, Y: R) -> Self {
324//! self.wXYZ[2] = Y;
325//! self
326//! }
327//! pub fn set_Z(mut self, Z: R) -> Self {
328//! self.wXYZ[3] = Z;
329//! self
330//! }
331//! }
332//!
333//! impl<R: Real> Default for Point3<R> {
334//! fn default() -> Self {
335//! Self::from([R::ONE, R::ZERO, R::ZERO, R::ZERO])
336//! }
337//! }
338//!
339//! impl<R: Real> From<[R; 4]> for Point3<R> {
340//! fn from(wXYZ: [R; 4]) -> Self {
341//! Self::from_wXYZ(wXYZ)
342//! }
343//! }
344//!
345//! impl<R: Real> Into<[R; 4]> for Point3<R> {
346//! fn into(self) -> [R; 4] {
347//! self.to_wXYZ()
348//! }
349//! }
350//!
351//! impl<R: Real> ApproxEq<R> for Point3<R> {
352//! fn approx_eq(&self, other: &Self, epsilon: R, ulp: R::Bits) -> bool {
353//! self.wXYZ.approx_eq(&other.wXYZ, epsilon, ulp)
354//! }
355//! }
356//!
357//! impl<R: Real> Add for Point3<R> {
358//! type Output = Self;
359//!
360//! fn add(self, other: Self) -> Self::Output {
361//! Self {
362//! wXYZ: self.wXYZ + other.wXYZ,
363//! }
364//! }
365//! }
366//!
367//! impl<R: Real> AddAssign for Point3<R> {
368//! fn add_assign(&mut self, other: Self) {
369//! *self = *self + other;
370//! }
371//! }
372//!
373//! impl<R: Real> Div<R> for Point3<R> {
374//! type Output = Self;
375//!
376//! fn div(self, other: R) -> Self::Output {
377//! Self {
378//! wXYZ: self.wXYZ / other.splat(),
379//! }
380//! }
381//! }
382//!
383//! impl<R: Real> DivAssign<R> for Point3<R> {
384//! fn div_assign(&mut self, other: R) {
385//! *self = *self / other;
386//! }
387//! }
388//!
389//! impl<R: Real> Mul<R> for Point3<R> {
390//! type Output = Self;
391//!
392//! fn mul(self, other: R) -> Self::Output {
393//! Self {
394//! wXYZ: self.wXYZ * other.splat(),
395//! }
396//! }
397//! }
398//!
399//! impl<R: Real> MulAssign<R> for Point3<R> {
400//! fn mul_assign(&mut self, other: R) {
401//! *self = *self * other;
402//! }
403//! }
404//!
405//! impl<R: Real> Neg for Point3<R> {
406//! type Output = Self;
407//!
408//! fn neg(self) -> Self::Output {
409//! Self { wXYZ: -self.wXYZ }
410//! }
411//! }
412//!
413//! impl<R: Real> Shl<Rotator3<R>> for Point3<R> {
414//! type Output = Self;
415//!
416//! fn shl(mut self, other: Rotator3<R>) -> Self::Output {
417//! other.point_fn()(&mut self);
418//! self
419//! }
420//! }
421//!
422//! impl<R: Real> ShlAssign<Rotator3<R>> for Point3<R> {
423//! fn shl_assign(&mut self, other: Rotator3<R>) {
424//! *self = *self << other
425//! }
426//! }
427//!
428//! impl<R: Real> Sub for Point3<R> {
429//! type Output = Self;
430//!
431//! fn sub(self, other: Self) -> Self::Output {
432//! Self {
433//! wXYZ: self.wXYZ - other.wXYZ,
434//! }
435//! }
436//! }
437//!
438//! impl<R: Real> SubAssign for Point3<R> {
439//! fn sub_assign(&mut self, other: Self) {
440//! *self = *self - other;
441//! }
442//! }
443//!
444//! let r000_ = Rotator3::default();
445//! let r030x = Rotator3::new(030f64.to_radians(), 1.0, 0.0, 0.0);
446//! let r060x = Rotator3::new(060f64.to_radians(), 1.0, 0.0, 0.0);
447//! let r330x = Rotator3::new(330f64.to_radians(), 1.0, 0.0, 0.0);
448//! assert!((r030x * r030x).approx_eq(&r060x, 0.0, 0));
449//! assert!((r030x * 42.0).unit().approx_eq(&r030x, 0.0, 0));
450//! assert!(((r030x * 42.0) * (r030x * 42.0).inv()).approx_eq(&r000_, f64::EPSILON, 0));
451//! assert!((r030x * r030x.rev()).approx_eq(&Rotator3::default(), f64::EPSILON, 0));
452//! assert!(r330x.constrain().approx_eq(&r030x.rev(), 0.0, 5));
453//!
454//! let r090x = Rotator3::new(090f64.to_radians(), 1.0, 0.0, 0.0);
455//! let x5 = Point3::new(1.0, 5.0, 0.0, 0.0);
456//! let y5 = Point3::new(1.0, 0.0, 5.0, 0.0);
457//! let z5 = Point3::new(1.0, 0.0, 0.0, 5.0);
458//! assert!((x5 << r090x).approx_eq(&x5, 0.0, 0));
459//! assert!((y5 << r090x).approx_eq(&z5, 5.0 * f64::EPSILON, 0));
460//! ```