pbrt_r3/core/
quaternion.rs

1use crate::core::base::*;
2use crate::core::transform::*;
3use std::ops;
4
5#[derive(Debug, PartialEq, Default, Copy, Clone)]
6pub struct Quaternion {
7    pub x: Float,
8    pub y: Float,
9    pub z: Float,
10    pub w: Float,
11}
12
13impl Quaternion {
14    pub fn new(x: Float, y: Float, z: Float, w: Float) -> Self {
15        if w >= 0.0 {
16            return Quaternion { x, y, z, w };
17        } else {
18            return Quaternion {
19                x: -x,
20                y: -y,
21                z: -z,
22                w: -w,
23            };
24        }
25    }
26
27    pub fn identity() -> Self {
28        Quaternion {
29            x: 0.0,
30            y: 0.0,
31            z: 0.0,
32            w: 1.0,
33        }
34    }
35
36    pub fn normalize(&self) -> Self {
37        let l = Float::sqrt(Quaternion::dot(self, self));
38        return Quaternion::new(self.x / l, self.y / l, self.z / l, self.w / l);
39    }
40
41    pub fn dot(&self, q2: &Quaternion) -> Float {
42        return (self.x * q2.x) + (self.y * q2.y) + (self.z * q2.z) + (self.w * q2.w);
43    }
44
45    pub fn slerp(t: Float, q1: &Quaternion, q2: &Quaternion) -> Quaternion {
46        const T: Float = 1.0 - Float::EPSILON;
47        let c = Self::dot(q1, q2).clamp(0.0, 1.0);
48        if c > T {
49            return *q1;
50        } else {
51            let theta = Float::acos(c);
52            let s = Float::recip(Float::sin(theta));
53            return ((*q1) * Float::sin((1.0 - t) * theta) + (*q2) * Float::sin(t * theta)) * s;
54        }
55    }
56
57    pub fn to_matrix(&self) -> Matrix4x4 {
58        let x = self.x;
59        let y = self.y;
60        let z = self.z;
61        let w = self.w;
62
63        let xx = x * x;
64        let yy = y * y;
65        let zz = z * z;
66        let xy = x * y;
67        let xz = x * z;
68        let yz = y * z;
69        let wx = x * w;
70        let wy = y * w;
71        let wz = z * w;
72
73        let mut m = Matrix4x4::identity();
74        m.m[4 * 0 + 0] = 1.0 - 2.0 * (yy + zz);
75        m.m[4 * 0 + 1] = 2.0 * (xy + wz);
76        m.m[4 * 0 + 2] = 2.0 * (xz - wy);
77        m.m[4 * 1 + 0] = 2.0 * (xy - wz);
78        m.m[4 * 1 + 1] = 1.0 - 2.0 * (xx + zz);
79        m.m[4 * 1 + 2] = 2.0 * (yz + wx);
80        m.m[4 * 2 + 0] = 2.0 * (xz + wy);
81        m.m[4 * 2 + 1] = 2.0 * (yz - wx);
82        m.m[4 * 2 + 2] = 1.0 - 2.0 * (xx + yy);
83        return m.transpose();
84    }
85
86    pub fn from_angle_axis(theta: Float, axis: &Vector3f) -> Self {
87        let theta = theta / 2.0;
88        let sin_theta = Float::sin(theta);
89        let cos_theta = Float::cos(theta);
90        let v = axis.normalize() * sin_theta;
91        return Quaternion::new(v.x, v.y, v.z, cos_theta);
92    }
93
94    /*
95    pub fn from_matrix(m: &Matrix4x4) -> Self {
96        let trace = m.m[0] + m.m[5] + m.m[10];
97        if trace > 0.0 {
98            // Compute w from matrix trace, then xyz
99            // 4w^2 = m[0][0] + m[1][1] + m[2][2] + m[3][3] (but m[3][3] == 1)
100            let s = Float::sqrt(trace + 1.0);
101            let w = s / 2.0;
102            let s = 0.5 / s;
103            let x = (m.m[4 * 2 + 1] - m.m[4 * 1 + 2]) * s; //21 12
104            let y = (m.m[4 * 0 + 2] - m.m[4 * 2 + 0]) * s; //02 20
105            let z = (m.m[4 * 1 + 0] - m.m[4 * 0 + 1]) * s; //10 01
106            return Quaternion::new(x, y, z, w);
107        } else {
108            // Compute largest of $x$, $y$, or $z$, then remaining components
109            let nxt = [1, 2, 0];
110            let mut q = [0.0; 3];
111            let mut i = 0;
112            if m.m[4 * 1 + 1] > m.m[4 * i + i] {
113                i = 1;
114            }
115            if m.m[4 * 2 + 2] > m.m[4 * i + i] {
116                i = 2;
117            }
118
119            let j = nxt[i];
120            let k = nxt[j];
121            let mut s = Float::sqrt((m.m[4 * i + i] - (m.m[4 * j + j] + m.m[4 * k + k])) + 1.0);
122            q[i] = s * 0.5;
123            if s != 0.0 {
124                s = 0.5 / s;
125            }
126            let w = (m.m[4 * k + j] - m.m[4 * j + k]) * s;
127            q[j] = (m.m[4 * j + i] + m.m[4 * i + j]) * s;
128            q[k] = (m.m[4 * k + i] + m.m[4 + i + k]) * s;
129
130            let x = q[0];
131            let y = q[1];
132            let z = q[2];
133            return Quaternion::new(x, y, z, w);
134        }
135    }
136    */
137    pub fn from_matrix(m: &Matrix4x4) -> Self {
138        let trace = m.m[0] + m.m[5] + m.m[10];
139        if trace > 0.0 {
140            // Compute w from matrix trace, then xyz
141            // 4w^2 = m[0][0] + m[1][1] + m[2][2] + m[3][3] (but m[3][3] == 1)
142            let s = Float::sqrt(trace + 1.0) * 2.0;
143            let x = (m.m[4 * 2 + 1] - m.m[4 * 1 + 2]) / s; //21 12
144            let y = (m.m[4 * 0 + 2] - m.m[4 * 2 + 0]) / s; //02 20
145            let z = (m.m[4 * 1 + 0] - m.m[4 * 0 + 1]) / s; //10 01
146            let w = s / 4.0;
147            return Quaternion::new(x, y, z, w);
148        } else if m.m[4 * 0 + 0] > m.m[4 * 1 + 1] && m.m[4 * 0 + 0] > m.m[4 * 2 + 2] {
149            let s = Float::sqrt(1.0 + m.m[4 * 0 + 0] - m.m[4 * 1 + 1] - m.m[4 * 2 + 2]) * 2.0;
150            let x = s / 4.0;
151            let y = (m.m[4 * 1 + 0] + m.m[4 * 0 + 1]) / s;
152            let z = (m.m[4 * 0 + 2] + m.m[4 * 2 + 0]) / s;
153            let w = (m.m[4 * 2 + 1] - m.m[4 * 1 + 2]) / s;
154            return Quaternion::new(x, y, z, w);
155        } else if m.m[4 * 1 + 1] > m.m[4 * 2 + 2] {
156            let s = Float::sqrt(1.0 + m.m[4 * 1 + 1] - m.m[4 * 0 + 0] - m.m[4 * 2 + 2]) * 2.0;
157            let x = (m.m[4 * 1 + 0] + m.m[4 * 0 + 1]) / s;
158            let y = s / 4.0;
159            let z = (m.m[4 * 2 + 1] + m.m[4 * 1 + 2]) / s;
160            let w = (m.m[4 * 0 + 2] - m.m[4 * 2 + 0]) / s;
161            return Quaternion::new(x, y, z, w);
162        } else {
163            let s = Float::sqrt(1.0 + m.m[4 * 2 + 2] - m.m[4 * 0 + 0] - m.m[4 * 1 + 1]) * 2.0;
164            let x = (m.m[4 * 0 + 2] + m.m[4 * 2 + 0]) / s;
165            let y = (m.m[4 * 2 + 1] + m.m[4 * 1 + 2]) / s;
166            let z = s / 4.0;
167            let w = (m.m[4 * 1 + 0] - m.m[4 * 0 + 1]) / s;
168            return Quaternion::new(x, y, z, w);
169        }
170    }
171}
172
173impl ops::Add<Quaternion> for Quaternion {
174    type Output = Quaternion;
175    fn add(self, rhs: Quaternion) -> Quaternion {
176        Quaternion {
177            x: self.x + rhs.x,
178            y: self.y + rhs.y,
179            z: self.z + rhs.z,
180            w: self.w + rhs.w,
181        }
182    }
183}
184
185impl ops::Sub<Quaternion> for Quaternion {
186    type Output = Quaternion;
187    fn sub(self, rhs: Quaternion) -> Quaternion {
188        Quaternion {
189            x: self.x - rhs.x,
190            y: self.y - rhs.y,
191            z: self.z - rhs.z,
192            w: self.w - rhs.w,
193        }
194    }
195}
196
197/*
198impl ops::Mul<Quaternion> for Quaternion {
199    type Output = Quaternion;
200    fn mul(self, rhs: Quaternion) -> Quaternion {
201        Quaternion {
202            x: self.x * rhs.x,
203            y: self.y * rhs.y,
204            z: self.z * rhs.z,
205            w: self.w * rhs.w,
206        }
207    }
208}
209*/
210
211impl ops::Mul<Float> for Quaternion {
212    type Output = Quaternion;
213    fn mul(self, rhs: Float) -> Quaternion {
214        Quaternion {
215            x: self.x * rhs,
216            y: self.y * rhs,
217            z: self.z * rhs,
218            w: self.w * rhs,
219        }
220    }
221}
222
223impl ops::Neg for Quaternion {
224    type Output = Quaternion;
225    fn neg(self) -> Quaternion {
226        return Quaternion {
227            x: -self.x,
228            y: -self.y,
229            z: -self.z,
230            w: -self.w,
231        };
232    }
233}
234
235impl From<Matrix4x4> for Quaternion {
236    fn from(m: Matrix4x4) -> Self {
237        return Quaternion::from_matrix(&m);
238    }
239}
pbrt_r3/core/quaternion.rs

pbrt_r3/core/
quaternion.rs
