siege_math/
quat.rs

1
2use std::ops::{Add, Sub, Mul, AddAssign, SubAssign, MulAssign};
3use num_traits::NumCast;
4use std::default::Default;
5use float_cmp::{Ulps, ApproxEq};
6use {FullFloat, Vec3, Mat3, Angle, Direction3};
7
8/// Quaternion (general)
9#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
10#[derive(Serialize, Deserialize)]
11pub struct Quat<F> {
12    pub v: Vec3<F>,
13    pub w: F
14}
15
16/// Normalized unit quaternion
17#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
18#[derive(Serialize, Deserialize)]
19pub struct NQuat<F> {
20    v: Vec3<F>,
21    w: F
22}
23
24impl<F: FullFloat> Quat<F> {
25    pub fn new(v: Vec3<F>, w: F) -> Quat<F> {
26        Quat {
27            v: v,
28            w: w,
29        }
30    }
31}
32
33impl<F: FullFloat> NQuat<F> {
34    pub fn new_isnormal(v: Vec3<F>, w: F) -> NQuat<F> {
35        let q = NQuat {
36            v: v,
37            w: w,
38        };
39
40        assert!((w*w + v.squared_magnitude()).sqrt().approx_eq(
41            &F::one(),
42            <F as NumCast>::from(20.0_f32).unwrap() * F::epsilon(),
43            NumCast::from(20_u32).unwrap(),
44        ));
45
46        q
47    }
48}
49
50impl<F: FullFloat> Quat<F> {
51    pub fn identity() -> Quat<F> {
52        Quat {
53            v: Vec3::new(F::zero(), F::zero(), F::zero()),
54            w: F::one()
55        }
56    }
57}
58impl<F: FullFloat> NQuat<F> {
59    pub fn identity() -> NQuat<F> {
60        NQuat::new_isnormal(
61            Vec3::new(F::zero(), F::zero(), F::zero()),
62            F::one())
63    }
64}
65
66impl<F: FullFloat> Default for Quat<F> {
67    fn default() -> Quat<F> {
68        Quat::identity()
69    }
70}
71
72impl<F: FullFloat> Default for NQuat<F> {
73    fn default() -> NQuat<F> {
74        NQuat::identity()
75    }
76}
77
78// ----------------------------------------------------------------------------
79// casting between float types
80
81impl From<Quat<f32>> for Quat<f64> {
82    fn from(q: Quat<f32>) -> Quat<f64> {
83        Quat {
84            v: From::from(q.v),
85            w: q.w as f64
86        }
87    }
88}
89
90impl From<Quat<f64>> for Quat<f32> {
91    fn from(q: Quat<f64>) -> Quat<f32> {
92        Quat {
93            v: From::from(q.v),
94            w: q.w as f32
95        }
96    }
97}
98
99impl From<NQuat<f32>> for NQuat<f64> {
100    fn from(q: NQuat<f32>) -> NQuat<f64> {
101        NQuat::new_isnormal(
102            From::from(q.v),
103            q.w as f64)
104    }
105}
106
107impl From<NQuat<f64>> for NQuat<f32> {
108    fn from(q: NQuat<f64>) -> NQuat<f32> {
109        NQuat::new_isnormal(
110            From::from(q.v),
111            q.w as f32)
112    }
113}
114
115// ----------------------------------------------------------------------------
116// Casting to/from normal form
117
118impl<F: FullFloat> From<Quat<F>> for NQuat<F>
119{
120    fn from(q: Quat<F>) -> NQuat<F> {
121        let mag = q.magnitude();
122        NQuat::new_isnormal(
123            q.v / mag,
124            q.w / mag)
125    }
126}
127
128impl<F: FullFloat> From<NQuat<F>> for Quat<F> {
129    fn from(nq: NQuat<F>) -> Quat<F> {
130        Quat { v: nq.v, w: nq.w }
131    }
132}
133
134// ----------------------------------------------------------------------------
135// Axis/Angle
136
137impl<F: FullFloat> NQuat<F> {
138    /// Create an NQuat from an axis and angle
139    // This always yields normal quats (tested)
140    pub fn from_axis_angle(axis: &Direction3<F>, angle: &Angle<F>) -> NQuat<F>
141    {
142        let two: F = NumCast::from(2.0_f32).unwrap();
143        let (s,c) = (angle.as_radians() / two).sin_cos();
144        let q = Quat {
145            v: Vec3::new(axis.x * s, axis.y * s, axis.z * s),
146            w: c
147        };
148        From::from(q)
149    }
150}
151
152impl<F: FullFloat> NQuat<F> {
153    /// Determine the axis/angle representation of an NQuat
154    pub fn as_axis_angle(&self) -> (Direction3<F>, Angle<F>)
155    {
156        let two: F = NumCast::from(2.0_f32).unwrap();
157        let angle = self.w.acos() * two;
158        let axis = self.v;
159        (From::from(axis), Angle::from_radians(angle))
160    }
161}
162
163// ----------------------------------------------------------------------------
164// Compute the quat that rotates from start to end
165
166impl<F: FullFloat> NQuat<F> {
167    // This returns None if start/end are the same or opposite)
168    pub fn from_directions(start: Direction3<F>, end: Direction3<F>) -> Option<NQuat<F>>
169    {
170        // If they are the same, we can return the identity quaternion
171        if start.approx_eq(&end,
172                           <F as NumCast>::from(10.0_f32).unwrap() * F::epsilon(),
173                           NumCast::from(10_u32).unwrap())
174        {
175            return Some(NQuat::identity())
176        }
177        // If they are opposite, there is no quaternion that will work.
178        if start.approx_eq(&-end,
179                           <F as NumCast>::from(10.0_f32).unwrap() * F::epsilon(),
180                           NumCast::from(10_u32).unwrap())
181        {
182            return None
183        }
184
185        let two: F = NumCast::from(2.0_f32).unwrap();
186        let e = start.dot(end);
187        if e==-F::one() {
188            // The input vectors are the same or opposite (although we should
189            // have detected this up above)
190            return None;
191        }
192        let term = (two * (F::one() + e)).sqrt();
193        let v: Vec3<F> = start.cross(end);
194        let q = Quat {
195            v: v / term,
196            w: term / two
197        };
198        // q is near-normal, but we require better (at some cost!)
199        Some(From::from(q))
200    }
201}
202
203// ----------------------------------------------------------------------------
204// Magnitude
205
206impl<F: FullFloat> Quat<F>
207{
208    pub fn squared_magnitude(&self) -> F {
209        self.w * self.w + self.v.squared_magnitude()
210    }
211}
212
213impl<F: FullFloat> Quat<F>
214{
215    pub fn magnitude(&self) -> F {
216        self.squared_magnitude().sqrt()
217    }
218}
219
220impl<F: FullFloat> Quat<F> {
221    pub fn is_normal(&self) -> bool {
222        self.magnitude().approx_eq(
223            &F::one(),
224            <F as NumCast>::from(10.0_f32).unwrap() * F::epsilon(),
225            NumCast::from(10_u32).unwrap()
226        )
227    }
228}
229
230// ----------------------------------------------------------------------------
231// Add/Sub
232
233impl<F: FullFloat> Add for Quat<F>
234{
235    type Output = Quat<F>;
236
237    fn add(self, rhs: Quat<F>) -> Quat<F> {
238        Quat {
239            v: self.v + rhs.v,
240            w: self.w + rhs.w,
241        }
242    }
243}
244
245impl<F: FullFloat> AddAssign for Quat<F>
246{
247    fn add_assign(&mut self, rhs: Quat<F>) {
248        self.v += rhs.v;
249        self.w += rhs.w;
250    }
251}
252
253impl<F: FullFloat> Sub for Quat<F>
254{
255    type Output = Quat<F>;
256
257    fn sub(self, rhs: Quat<F>) -> Quat<F> {
258        Quat {
259            v: self.v - rhs.v,
260            w: self.w - rhs.w,
261        }
262    }
263}
264
265impl<F: FullFloat> SubAssign for Quat<F>
266{
267    fn sub_assign(&mut self, rhs: Quat<F>) {
268        self.v -= rhs.v;
269        self.w -= rhs.w;
270    }
271}
272
273// ----------------------------------------------------------------------------
274// Scalar Mul, Dot product, Hamiltonian product
275
276impl<F: FullFloat> Mul<F> for Quat<F>
277{
278    type Output = Quat<F>;
279
280    fn mul(self, rhs: F) -> Quat<F> {
281        Quat {
282            v: self.v * rhs,
283            w: self.w * rhs,
284        }
285    }
286}
287
288impl<F: FullFloat> MulAssign<F> for Quat<F>
289{
290    fn mul_assign(&mut self, rhs: F) {
291        self.v *= rhs;
292        self.w *= rhs;
293    }
294}
295
296impl<F: FullFloat> MulAssign for Quat<F>
297{
298    fn mul_assign(&mut self, rhs: Quat<F>) {
299        self.v = self.v.cross(rhs.v)  +  rhs.v * self.w  +  self.v * rhs.w;
300        self.w = self.w * rhs.w  -  self.v.dot(rhs.v);
301    }
302}
303
304impl<F: FullFloat> Quat<F>
305{
306    pub fn dot(&self, other: Quat<F>) -> F
307    {
308        (self.w * other.w) + self.v.dot(other.v)
309    }
310}
311
312impl<F: FullFloat> NQuat<F>
313{
314    pub fn dot(&self, other: Quat<F>) -> F
315    {
316        (self.w * other.w) + self.v.dot(other.v)
317    }
318}
319
320impl<F: FullFloat> Mul for Quat<F>
321{
322    type Output = Quat<F>;
323
324    fn mul(self, rhs: Quat<F>) -> Quat<F> {
325        Quat {
326            v: self.v.cross(rhs.v)  +  rhs.v * self.w  +  self.v * rhs.w,
327            w: self.w * rhs.w  -  self.v.dot(rhs.v)
328        }
329    }
330}
331
332impl<F: FullFloat> Mul for NQuat<F>
333{
334    type Output = NQuat<F>;
335
336    fn mul(self, rhs: NQuat<F>) -> NQuat<F> {
337        let a: Quat<F> = From::from(self);
338        let b: Quat<F> = From::from(rhs);
339        From::from(a * b)
340    }
341}
342
343// ----------------------------------------------------------------------------
344// Conjugate
345
346impl<F: FullFloat> Quat<F>
347{
348    pub fn conjugate(&self) -> Quat<F> {
349        Quat {
350            v: -self.v,
351            w: self.w
352        }
353    }
354}
355
356impl<F: FullFloat> NQuat<F>
357{
358    pub fn conjugate(&self) -> NQuat<F> {
359        NQuat::new_isnormal(
360            -self.v,
361            self.w)
362    }
363}
364
365// ----------------------------------------------------------------------------
366// Rotate a vector
367
368impl<F: FullFloat> NQuat<F>
369{
370    /// This applies the NQuat as an operator to the vector v, yielding the rotated
371    /// vector.
372    // In general, the sandwich product qvq* does not care if Q is normalized or not.
373    // However we presume it is normalized so we can take some shortcuts.
374    // See Eric Lengyel, Foundations of Game Engine Development: Vol.1 Mathematics, p89
375    pub fn rotate(&self, v: Vec3<F>) -> Vec3<F> {
376        let dot = v.dot(self.v);
377
378        v * ((self.w * self.w) - (self.v.x * self.v.x) - (self.v.y * self.v.y) - (self.v.z * self.v.z))
379            + self.v * (dot + dot)
380            + self.v.cross(v) * (self.w + self.w)
381    }
382}
383
384// implemented also as NQuat * Vec3
385impl<F: FullFloat> Mul<Vec3<F>> for NQuat<F>
386{
387    type Output = Vec3<F>;
388
389    fn mul(self, rhs: Vec3<F>) -> Vec3<F> {
390        self.rotate(rhs)
391    }
392}
393
394// ----------------------------------------------------------------------------
395// To/From Matrix
396
397impl<F: FullFloat> From<NQuat<F>> for Mat3<F> {
398    fn from(q: NQuat<F>) -> Mat3<F> {
399        let two: F = NumCast::from(2.0_f32).unwrap();
400        let x2 = q.v.x * q.v.x;
401        let y2 = q.v.y * q.v.y;
402        let z2 = q.v.z * q.v.z;
403        let xy = q.v.x * q.v.y;
404        let xz = q.v.x * q.v.z;
405        let yz = q.v.y * q.v.z;
406        let wx = q.w * q.v.x;
407        let wy = q.w * q.v.y;
408        let wz = q.w * q.v.z;
409
410        Mat3::new(
411            F::one() - two * (y2 + z2),             two * (xy - wz),            two * (xz + wy),
412            two            * (xy + wz),  F::one() - two * (x2 + z2),            two * (yz - wx),
413            two            * (xz - wy),             two * (yz + wx), F::one() - two * (x2 + y2)
414        )
415    }
416}
417
418impl<F: FullFloat> From<Mat3<F>> for NQuat<F> {
419    fn from(m: Mat3<F>) -> NQuat<F> {
420        let one: F = F::one();
421        let half: F = NumCast::from(0.5_f32).unwrap();
422        let quarter: F = NumCast::from(0.25_f32).unwrap();
423
424        let sum = m.x.x + m.y.y + m.z.z;
425        let x;
426        let y;
427        let z;
428        let w;
429        if sum>F::zero() {
430            w = (sum + one).sqrt() * half;
431            let f = quarter / w;
432            x = (m.z.y - m.y.z) * f;
433            y = (m.x.z - m.z.x) * f;
434            z = (m.y.x - m.x.y) * f;
435        }
436        else if (m.x.x > m.y.y) && (m.x.x > m.z.z) {
437            x = (m.x.x - m.y.y - m.z.z + one).sqrt() * half;
438            let f = quarter / x;
439            y = (m.y.x + m.x.y) * f;
440            z = (m.x.z + m.z.x) * f;
441            w = (m.z.y - m.y.z) * f;
442        }
443        else if m.y.y > m.z.z {
444            y = (m.y.y - m.x.x - m.z.z + one).sqrt() * half;
445            let f = quarter / y;
446            x = (m.y.x + m.x.y) * f;
447            z = (m.z.y + m.y.z) * f;
448            w = (m.x.z - m.z.x) * f;
449        }
450        else {
451            z = (m.z.z - m.x.x - m.y.y + one).sqrt() * half;
452            let f = quarter / z;
453            x = (m.x.z + m.z.x) * f;
454            y = (m.z.y + m.y.z) * f;
455            w = (m.y.x - m.x.y) * f;
456        }
457
458        let q = Quat {
459            v: Vec3 { x: x, y: y, z: z },
460            w: w
461        };
462
463        // The above is theoretically normal, but instability will probably
464        // put it far off in Ulps terms. So we normalize
465        From::from(q)
466    }
467}
468
469// ----------------------------------------------------------------------------
470// ApproxEq
471
472impl<F: FullFloat> ApproxEq for Quat<F> {
473    type Flt = F;
474
475    fn approx_eq(&self, other: &Self,
476                 epsilon: <F as ApproxEq>::Flt,
477                 ulps: <<F as ApproxEq>::Flt as Ulps>::U) -> bool
478    {
479        self.v.approx_eq(&other.v, epsilon, ulps) &&
480            self.w.approx_eq(&other.w, epsilon, ulps)
481    }
482}
483
484impl<F: FullFloat> ApproxEq for NQuat<F> {
485    type Flt = F;
486
487    fn approx_eq(&self, other: &Self,
488                 epsilon: <F as ApproxEq>::Flt,
489                 ulps: <<F as ApproxEq>::Flt as Ulps>::U) -> bool
490    {
491        self.v.approx_eq(&other.v, epsilon, ulps) &&
492            self.w.approx_eq(&other.w, epsilon, ulps)
493    }
494}
495
496// ----------------------------------------------------------------------------
497
498#[cfg(test)]
499mod tests {
500    use {Quat, NQuat, Vec3, Mat3, Angle, Direction3};
501
502    #[test]
503    fn test_quat_basic() {
504        let q = Quat::<f64>::new(
505            Vec3::<f64>::new(3.0, 4.0, 5.0),
506            6.0);
507        assert_eq!(q.v.x, 3.0);
508        assert_eq!(q.v.y, 4.0);
509        assert_eq!(q.v.z, 5.0);
510        assert_eq!(q.w, 6.0);
511
512        let q = Quat::<f64>::identity();
513        assert_eq!(q.v.x, 0.0);
514        assert_eq!(q.v.y, 0.0);
515        assert_eq!(q.v.z, 0.0);
516        assert_eq!(q.w, 1.0);
517
518        let q2 = Quat::<f64>::default();
519        assert_eq!(q, q2);
520
521        let q: NQuat<f64> = From::from(
522            Quat::<f64>::new(Vec3::<f64>::new(3.0, 4.0, 5.0), 6.0)
523        );
524        let q2: Quat<f64> = From::from(q);
525        assert!(0.999999999 < q2.magnitude());
526        assert!(q2.magnitude() < 1.000000001);
527    }
528
529    #[test]
530    fn test_quat_mat_conversion() {
531        use float_cmp::ApproxEq;
532
533        let v = Vec3::<f32>::new(1.0, 0.2, -0.3);
534        let q = Quat::<f32>::new(v, 5.0);
535        let nq: NQuat<f32> = From::from(q);
536        let q: Quat<f32> = From::from(nq);
537
538        let m: Mat3<f32> = From::from(nq);
539
540        // Conversion through a matrix could return the
541        // conjugate (which represents the same rotation)
542        // so we have to check for either
543        let nq: NQuat<f32> = From::from(m);
544        let q2: Quat<f32> = From::from(nq);
545        let q2c: Quat<f32> = q2.conjugate();
546
547        assert!(q2.approx_eq(&q, 2.0 * ::std::f32::EPSILON, 2) ||
548                q2c.approx_eq(&q, 2.0 * ::std::f32::EPSILON, 2));
549    }
550
551    #[test]
552    fn test_axis_angle() {
553        use float_cmp::ApproxEq;
554
555        let axis: Direction3<f32> = From::from(Vec3::<f32>::new(1.0, 1.0, 1.0));
556        let angle = Angle::<f32>::from_degrees(90.0);
557        let q = NQuat::<f32>::from_axis_angle(&axis, &angle);
558        let (axis2, angle2) = q.as_axis_angle();
559        println!("axis {:?} angle {:?} axis {:?} angle {:?}",
560                 axis, angle, axis2, angle2);
561        assert!(axis.approx_eq(&axis2, 2.0 * ::std::f32::EPSILON, 2));
562        assert!(angle.approx_eq(&angle2, 2.0 * ::std::f32::EPSILON, 2));
563    }
564
565    #[test]
566    fn test_rotation_x() {
567        use float_cmp::ApproxEq;
568
569        let axis: Direction3<f32> = From::from(Vec3::<f32>::new(1.0, 0.0, 0.0));
570        let angle = Angle::<f32>::from_degrees(90.0);
571        let q = NQuat::<f32>::from_axis_angle(&axis, &angle);
572
573        let object = Vec3::<f32>::new(10.0, 5.0, 3.0);
574
575        let object2 = q.rotate(object);
576
577        assert!(object2.x.approx_eq(&10.0, 2.0 * ::std::f32::EPSILON, 2));
578        assert!(object2.y.approx_eq(&-3.0, 2.0 * ::std::f32::EPSILON, 2));
579        assert!(object2.z.approx_eq(&5.0, 2.0 * ::std::f32::EPSILON, 2));
580    }
581
582    /*
583    #[test]
584    fn test_normal_or_not() {
585        // This was for me to determine some things.
586        let axis: Direction3<f32> = From::from(Vec3::<f32>::new(1.0, 5.0, 6.0003));
587        let angle = Angle::<f32>::from_degrees(93.4);
588        let q = NQuat::<f32>::from_axis_angle(&axis, &angle);
589
590        if q.is_normal() {
591            println!("from_axis_angle yields normal quats");
592        } else {
593            println!("from_axis_angle yields general quats");
594        }
595    }
596    */
597}
siege_math/quat.rs

siege_math/
quat.rs
