1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146

use std::fmt;
use approx::ApproxEq;
use num_complex::Complex;

use alga::general::Real;
use core::{Unit, Vector1, Matrix2, Matrix3};
use geometry::Rotation2;

/// A complex number with a norm equal to 1.
pub type UnitComplex<N> = Unit<Complex<N>>;

impl<N: Real> UnitComplex<N> {
    /// The rotation angle in `]-pi; pi]` of this unit complex number.
    #[inline]
    pub fn angle(&self) -> N {
        self.im.atan2(self.re)
    }

    /// The sine of the rotation angle.
    #[inline]
    pub fn sin_angle(&self) -> N {
        self.im
    }

    /// The cosine of the rotation angle.
    #[inline]
    pub fn cos_angle(&self) -> N {
        self.re
    }

    /// The rotation angle returned as a 1-dimensional vector.
    #[inline]
    pub fn scaled_axis(&self) -> Vector1<N> {
        Vector1::new(self.angle())
    }

    /// The underlying complex number.
    ///
    /// Same as `self.as_ref()`.
    #[inline]
    pub fn complex(&self) -> &Complex<N> {
        self.as_ref()
    }

    /// Compute the conjugate of this unit complex number.
    #[inline]
    pub fn conjugate(&self) -> Self {
        UnitComplex::new_unchecked(self.conj())
    }

    /// Inverts this complex number if it is not zero.
    #[inline]
    pub fn inverse(&self) -> Self {
        self.conjugate()
    }

    /// The rotation angle needed to make `self` and `other` coincide.
    #[inline]
    pub fn angle_to(&self, other: &Self) -> N {
        let delta = self.rotation_to(other);
        delta.angle()
    }

    /// The unit complex number needed to make `self` and `other` coincide.
    ///
    /// The result is such that: `self.rotation_to(other) * self == other`.
    #[inline]
    pub fn rotation_to(&self, other: &Self) -> Self {
        other / self
    }

    /// Compute in-place the conjugate of this unit complex number.
    #[inline]
    pub fn conjugate_mut(&mut self) {
        let me = self.as_mut_unchecked();
        me.im = -me.im;
    }

    /// Inverts in-place this unit complex number.
    #[inline]
    pub fn inverse_mut(&mut self) {
        self.conjugate_mut()
    }

    /// Raise this unit complex number to a given floating power.
    ///
    /// This returns the unit complex number that identifies a rotation angle equal to
    /// `self.angle() × n`.
    #[inline]
    pub fn powf(&self, n: N) -> Self {
        Self::from_angle(self.angle() * n)
    }

    /// Builds the rotation matrix corresponding to this unit complex number.
    #[inline]
    pub fn to_rotation_matrix(&self) -> Rotation2<N> {
        let r = self.re;
        let i = self.im;

        Rotation2::from_matrix_unchecked(Matrix2::new(r, -i,
                                                      i,  r))
    }

    /// Converts this unit complex number into its equivalent homogeneous transformation matrix.
    #[inline]
    pub fn to_homogeneous(&self) -> Matrix3<N> {
        self.to_rotation_matrix().to_homogeneous()
    }
}

impl<N: Real + fmt::Display> fmt::Display for UnitComplex<N> {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "UnitComplex angle: {}", self.angle())
    }
}

impl<N: Real> ApproxEq for UnitComplex<N> {
    type Epsilon = N;

    #[inline]
    fn default_epsilon() -> Self::Epsilon {
        N::default_epsilon()
    }

    #[inline]
    fn default_max_relative() -> Self::Epsilon {
        N::default_max_relative()
    }

    #[inline]
    fn default_max_ulps() -> u32 {
        N::default_max_ulps()
    }

    #[inline]
    fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
        self.re.relative_eq(&other.re, epsilon, max_relative) &&
        self.im.relative_eq(&other.im, epsilon, max_relative)
    }

    #[inline]
    fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
        self.re.ulps_eq(&other.re, epsilon, max_ulps) &&
        self.im.ulps_eq(&other.im, epsilon, max_ulps)
    }
}