Add optimized routines for manipulating directions (ideal points)

Author: jeremyong

docs/api/kln::direction.md

@@ -5,25 +5,33 @@ struct kln::direction
   : public kln::entity< 0b1000 >
 ```  
 
+Directions in $\mathbf{P}(\mathbb{R}^3_{3, 0, 1})$ are represented using points at infinity (homogeneous coordinate 0). Having a homogeneous coordinate of zero ensures that directions are translation-invariant.
+
 ### Summary
 
  Members                        | Descriptions                                
 --------------------------------|---------------------------------------------
 `public  ` [`direction`](#structkln_1_1direction_1a6d22510e72f67516a65a2ee39a5591a3)`() = default`  | 
-`public  ` [`direction`](#structkln_1_1direction_1a8fc796733d32a69230136b8376003fca)`(float x,float y,float z) noexcept`  | 
+`public  ` [`direction`](#structkln_1_1direction_1a8fc796733d32a69230136b8376003fca)`(float x,float y,float z) noexcept`  | Create a normalized direction.
 `public  ` [`direction`](#structkln_1_1direction_1a81cd76b26d928f02e830d8c0175157d6)`(` [`entity`](/Klein/api/kln::entity#structkln_1_1entity)`< 0b1000 > const & e) noexcept`  | 
 `public constexpr float ` [`operator[]`](#structkln_1_1direction_1a8fa09342bff2583a7f29d04adf6dd3c2)`(size_t i) const noexcept`  | 
 `public constexpr float & ` [`operator[]`](#structkln_1_1direction_1a67a82ca405563ca75b7f8a84842e42c0)`(size_t i) noexcept`  | 
 `public float ` [`x`](#structkln_1_1direction_1a721087c2056a33cd780c678ae0ec39dd)`() const noexcept`  | 
+`public float & ` [`x`](#structkln_1_1direction_1a161c9821553a28babae050e37a0d1096)`() noexcept`  | 
 `public float ` [`y`](#structkln_1_1direction_1a40bc7052a639d57afde7a860c1631638)`() const noexcept`  | 
+`public float & ` [`y`](#structkln_1_1direction_1a582345212cc5165ac7f3b1c5483dd17c)`() noexcept`  | 
 `public float ` [`z`](#structkln_1_1direction_1a1e16d3ed0ff945d0d1cbf1e6458d334b)`() const noexcept`  | 
+`public float & ` [`z`](#structkln_1_1direction_1aff096710eaf0b26832f00b5313bdac75)`() noexcept`  | 
+`public void ` [`normalize`](#structkln_1_1direction_1a143cd2cfeb94860ce1d47b9735064802)`() noexcept`  | Normalize this direction by dividing all components by the square magnitude
 
 ### Members
 
 ####   [direction](#structkln_1_1direction_1a6d22510e72f67516a65a2ee39a5591a3)() = default  {#structkln_1_1direction_1a6d22510e72f67516a65a2ee39a5591a3}
 
 ####   [direction](#structkln_1_1direction_1a8fc796733d32a69230136b8376003fca)(float x,float y,float z) noexcept  {#structkln_1_1direction_1a8fc796733d32a69230136b8376003fca}
 
+Create a normalized direction.
+
 ####   [direction](#structkln_1_1direction_1a81cd76b26d928f02e830d8c0175157d6)( [entity](/Klein/api/kln::entity#structkln_1_1entity)< 0b1000 > const & e) noexcept  {#structkln_1_1direction_1a81cd76b26d928f02e830d8c0175157d6}
 
 #### float  [operator[]](#structkln_1_1direction_1a8fa09342bff2583a7f29d04adf6dd3c2)(size_t i) const noexcept  {#structkln_1_1direction_1a8fa09342bff2583a7f29d04adf6dd3c2}
@@ -32,7 +40,22 @@ struct kln::direction
 
 #### float  [x](#structkln_1_1direction_1a721087c2056a33cd780c678ae0ec39dd)() const noexcept  {#structkln_1_1direction_1a721087c2056a33cd780c678ae0ec39dd}
 
+#### float &  [x](#structkln_1_1direction_1a161c9821553a28babae050e37a0d1096)() noexcept  {#structkln_1_1direction_1a161c9821553a28babae050e37a0d1096}
+
 #### float  [y](#structkln_1_1direction_1a40bc7052a639d57afde7a860c1631638)() const noexcept  {#structkln_1_1direction_1a40bc7052a639d57afde7a860c1631638}
 
+#### float &  [y](#structkln_1_1direction_1a582345212cc5165ac7f3b1c5483dd17c)() noexcept  {#structkln_1_1direction_1a582345212cc5165ac7f3b1c5483dd17c}
+
 #### float  [z](#structkln_1_1direction_1a1e16d3ed0ff945d0d1cbf1e6458d334b)() const noexcept  {#structkln_1_1direction_1a1e16d3ed0ff945d0d1cbf1e6458d334b}
 
+#### float &  [z](#structkln_1_1direction_1aff096710eaf0b26832f00b5313bdac75)() noexcept  {#structkln_1_1direction_1aff096710eaf0b26832f00b5313bdac75}
+
+#### void  [normalize](#structkln_1_1direction_1a143cd2cfeb94860ce1d47b9735064802)() noexcept  {#structkln_1_1direction_1a143cd2cfeb94860ce1d47b9735064802}
+
+Normalize this direction by dividing all components by the square magnitude
+
+!!! tip 
+    Point normalization divides the coordinates by the quantity
+    a^2 + b^2 + c^2. This is done using the `rcpps` instruction with a
+    maximum relative error of $1.5\times 2^{-12}$.
+

docs/api/kln::motor.md

@@ -7,6 +7,42 @@ struct kln::motor
 
 A `motor`  represents a kinematic motion in our algebra. From [Chasles' theorem](https://en.wikipedia.org/wiki/Chasles%27_theorem_(kinematics)), we know that any rigid body displacement can be produced by a translation along a line, followed or preceded by a rotation about an axis parallel to that line. The motor algebra is isomorphic to the dual quaternions but exists here in the same algebra as all the other geometric entities and actions at our disposal. Operations such as composing a motor with a rotor or translator are possible for example. The primary benefit to using a motor over its corresponding matrix operation is twofold. First, you get the benefit of numerical stability when composing multiple actions via the geometric product (`*` ). Second, because the motors constitute a continuous group, they are amenable to smooth interpolation and differentiation.
 
+!!! example 
+    ```c++
+        // Create a rotor representing a pi/2 rotation about the z-axis
+        // Normalization is done automatically
+        rotor r{M_PI * 0.5f, 0.f, 0.f, 1.f};
+    
+        // Create a translator that represents a translation of 1 unit
+        // in the yz-direction. Normalization is done automatically.
+        translator t{1.f, 0.f, 1.f, 1.f};
+    
+        // Create a motor that combines the action of the rotation and
+        // translation above.
+        motor m = r * t;
+    
+        // Initialize a point at (1, 3, 2)
+        kln::point p1{1.f, 3.f, 2.f};
+    
+        // Translate p1 and rotate it to create a new point p2
+        kln::point p2 = m(p1);
+    ```
+    
+
+Motors can be multiplied to one another with the `*`  operator to create a new motor equivalent to the application of each factor.
+
+!!! example 
+    ```c++
+        // Suppose we have 3 motors m1, m2, and m3
+    
+        // The motor m created here represents the combined action of m1,
+        // m2, and m3.
+        kln::motor m = m3 * m2 * m1;
+    ```
+    
+
+The same `*`  operator can be used to compose the motor's action with other translators and rotors.
+
 A demonstration of using the exponential and logarithmic map to blend between two motors is provided in a test case [here](https://github.com/jeremyong/Klein/blob/master/test/test_exp_log.cpp#L48).
 
 ### Summary
@@ -27,6 +63,8 @@ A demonstration of using the exponential and logarithmic map to blend between tw
 `public ` [`point`](/Klein/api/kln::point#structkln_1_1point)` KLN_VEC_CALL ` [`operator()`](#structkln_1_1motor_1a7ae8d73c558d1f6581df3a4b50fb2a40)`(` [`point`](/Klein/api/kln::point#structkln_1_1point)` const & p) const noexcept`  | Conjugates a point $p$ with this motor and returns the result $mp\widetilde{m}$.
 `public void KLN_VEC_CALL ` [`operator()`](#structkln_1_1motor_1ab14cf440bf8281b4a56cc338c568156b)`(` [`point`](/Klein/api/kln::point#structkln_1_1point)` * in,` [`point`](/Klein/api/kln::point#structkln_1_1point)` * out,size_t count) const noexcept`  | Conjugates an array of points with this motor in the input array and stores the result in the output array. Aliasing is only permitted when `in == out`  (in place motor application).
 `public ` [`point`](/Klein/api/kln::point#structkln_1_1point)` KLN_VEC_CALL ` [`operator()`](#structkln_1_1motor_1afa77e3a1d5a8f28bf6eee4de9e174489)`(` [`origin`](/Klein/api/kln::origin#structkln_1_1origin)`) const noexcept`  | Conjugates the origin $O$ with this motor and returns the result $mO\widetilde{m}$.
+`public ` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` KLN_VEC_CALL ` [`operator()`](#structkln_1_1motor_1ac8debbfe23b80affa7bf9ef7e0dffc1f)`(` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` const & d) const noexcept`  | Conjugates a direction $d$ with this motor and returns the result $md\widetilde{m}$.
+`public void KLN_VEC_CALL ` [`operator()`](#structkln_1_1motor_1aff385bad1df3b7ee29439b56bec43376)`(` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` * in,` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` * out,size_t count) const noexcept`  | Conjugates an array of directions with this motor in the input array and stores the result in the output array. Aliasing is only permitted when `in == out`  (in place motor application).
 
 ### Members
 
@@ -97,3 +135,20 @@ Conjugates an array of points with this motor in the input array and stores the
 
 Conjugates the origin $O$ with this motor and returns the result $mO\widetilde{m}$.
 
+####  [direction](/Klein/api/kln::direction#structkln_1_1direction) KLN_VEC_CALL  [operator()](#structkln_1_1motor_1ac8debbfe23b80affa7bf9ef7e0dffc1f)( [direction](/Klein/api/kln::direction#structkln_1_1direction) const & d) const noexcept  {#structkln_1_1motor_1ac8debbfe23b80affa7bf9ef7e0dffc1f}
+
+Conjugates a direction $d$ with this motor and returns the result $md\widetilde{m}$.
+
+The cost of this operation is the same as the application of a rotor due to the translational invariance of directions (points at infinity).
+
+#### void KLN_VEC_CALL  [operator()](#structkln_1_1motor_1aff385bad1df3b7ee29439b56bec43376)( [direction](/Klein/api/kln::direction#structkln_1_1direction) * in, [direction](/Klein/api/kln::direction#structkln_1_1direction) * out,size_t count) const noexcept  {#structkln_1_1motor_1aff385bad1df3b7ee29439b56bec43376}
+
+Conjugates an array of directions with this motor in the input array and stores the result in the output array. Aliasing is only permitted when `in == out`  (in place motor application).
+
+The cost of this operation is the same as the application of a rotor due to the translational invariance of directions (points at infinity).
+
+!!! tip 
+    When applying a motor to a list of tightly packed directions, this
+    routine will be *significantly faster* than applying the motor to
+    each direction individually.
+

docs/api/kln::rotor.md

@@ -5,6 +5,44 @@ struct kln::rotor
   : public kln::entity< 0b10 >
 ```  
 
+The rotor is an entity that represents a rigid rotation about an axis. To apply the rotor to a supported entity, the call operator is available.
+
+!!! example 
+    ```c++
+        // Initialize a point at (1, 3, 2)
+        kln::point p{1.f, 3.f, 2.f};
+    
+        // Create a normalized rotor representing a pi/2 radian
+        // rotation about the xz-axis.
+        kln::rotor r{M_PI * 0.5f, 1.f, 0.f, 1.f};
+    
+        // Rotate our point using the created rotor
+        kln::point rotated = r(p);
+    ```
+    We can rotate lines and planes as well using the rotor's call operator.
+    
+
+Rotors can be multiplied to one another with the `*`  operator to create a new rotor equivalent to the application of each factor.
+
+!!! example 
+    ```c++
+        // Create a normalized rotor representing a $\frac{\pi}{2}$ radian
+        // rotation about the xz-axis.
+        kln::rotor r1{M_PI * 0.5f, 1.f, 0.f, 1.f};
+    
+        // Create a second rotor representing a $\frac{\pi}{3}$ radian
+        // rotation about the yz-axis.
+        kln::rotor r2{M_PI / 3.f, 0.f, 1.f, 1.f};
+    
+        // Use the geometric product to create a rotor equivalent to first
+        // applying r1, then applying r2. Note that the order of the
+        // operands here is significant.
+        kln::rotor r3 = r2 * r1;
+    ```
+    
+
+The same `*`  operator can be used to compose the rotor's action with other translators and motors.
+
 ### Summary
 
  Members                        | Descriptions                                
@@ -21,6 +59,8 @@ struct kln::rotor
 `public void KLN_VEC_CALL ` [`operator()`](#structkln_1_1rotor_1aad794881fa0c11fb05486986032a431a)`(` [`line`](/Klein/api/kln::line#structkln_1_1line)` * in,` [`line`](/Klein/api/kln::line#structkln_1_1line)` * out,size_t count) const noexcept`  | Conjugates an array of lines with this rotor in the input array and stores the result in the output array. Aliasing is only permitted when `in == out`  (in place rotor application).
 `public ` [`point`](/Klein/api/kln::point#structkln_1_1point)` KLN_VEC_CALL ` [`operator()`](#structkln_1_1rotor_1a5aabb4caa402fb5793807fe1d8cec199)`(` [`point`](/Klein/api/kln::point#structkln_1_1point)` const & p) const noexcept`  | Conjugates a point $p$ with this rotor and returns the result $rp\widetilde{r}$.
 `public void KLN_VEC_CALL ` [`operator()`](#structkln_1_1rotor_1aa91e4024d7368fbd14a93ead29905aad)`(` [`point`](/Klein/api/kln::point#structkln_1_1point)` * in,` [`point`](/Klein/api/kln::point#structkln_1_1point)` * out,size_t count) const noexcept`  | Conjugates an array of points with this rotor in the input array and stores the result in the output array. Aliasing is only permitted when `in == out`  (in place rotor application).
+`public ` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` KLN_VEC_CALL ` [`operator()`](#structkln_1_1rotor_1a80935add9a99987a59879df1aa868b43)`(` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` const & d) const noexcept`  | Conjugates a direction $d$ with this rotor and returns the result $rd\widetilde{r}$.
+`public void KLN_VEC_CALL ` [`operator()`](#structkln_1_1rotor_1a185ca5be93f00396e5e0de9a05561999)`(` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` * in,` [`direction`](/Klein/api/kln::direction#structkln_1_1direction)` * out,size_t count) const noexcept`  | Conjugates an array of directions with this rotor in the input array and stores the result in the output array. Aliasing is only permitted when `in == out`  (in place rotor application).
 
 ### Members
 
@@ -89,3 +129,16 @@ Conjugates an array of points with this rotor in the input array and stores the
     routine will be *significantly faster* than applying the rotor to
     each point individually.
 
+####  [direction](/Klein/api/kln::direction#structkln_1_1direction) KLN_VEC_CALL  [operator()](#structkln_1_1rotor_1a80935add9a99987a59879df1aa868b43)( [direction](/Klein/api/kln::direction#structkln_1_1direction) const & d) const noexcept  {#structkln_1_1rotor_1a80935add9a99987a59879df1aa868b43}
+
+Conjugates a direction $d$ with this rotor and returns the result $rd\widetilde{r}$.
+
+#### void KLN_VEC_CALL  [operator()](#structkln_1_1rotor_1a185ca5be93f00396e5e0de9a05561999)( [direction](/Klein/api/kln::direction#structkln_1_1direction) * in, [direction](/Klein/api/kln::direction#structkln_1_1direction) * out,size_t count) const noexcept  {#structkln_1_1rotor_1a185ca5be93f00396e5e0de9a05561999}
+
+Conjugates an array of directions with this rotor in the input array and stores the result in the output array. Aliasing is only permitted when `in == out`  (in place rotor application).
+
+!!! tip 
+    When applying a rotor to a list of tightly packed directions, this
+    routine will be *significantly faster* than applying the rotor to
+    each direction individually.
+

docs/api/kln::translator.md

@@ -5,6 +5,38 @@ struct kln::translator
   : public kln::entity< 0b110 >
 ```  
 
+A translator represents a rigid-body displacement along a normalized axis. To apply the translator to a supported entity, the call operator is available.
+
+!!! example 
+    ```c++
+        // Initialize a point at (1, 3, 2)
+        kln::point p{1.f, 3.f, 2.f};
+    
+        // Create a normalized translator representing a 4-unit
+        // displacement along the xz-axis.
+        kln::translator r{4.f, 1.f, 0.f, 1.f};
+    
+        // Displace our point using the created translator
+        kln::point translated = r(p);
+    ```
+    We can translate lines and planes as well using the translator's call
+    operator.
+    
+
+Translators can be multiplied to one another with the `*`  operator to create a new translator equivalent to the application of each factor.
+
+!!! example 
+    ```c++
+        // Suppose we have 3 translators t1, t2, and t3
+    
+        // The translator t created here represents the combined action of
+        // t1, t2, and t3.
+        kln::translator t = t3 * t2 * t1;
+    ```
+    
+
+The same `*`  operator can be used to compose the translator's action with other rotors and motors.
+
 ### Summary
 
  Members                        | Descriptions                                

public/klein/direction.hpp

@@ -4,14 +4,18 @@
 
 namespace kln
 {
-// Ideal points will have a 0 for the homogeneous coordinate
-// x*e_032 + y*e_013 + z*e_021
+/// Directions in $\mathbf{P}(\mathbb{R}^3_{3, 0, 1})$ are represented using
+/// points at infinity (homogeneous coordinate 0). Having a homogeneous
+/// coordinate of zero ensures that directions are translation-invariant.
 struct direction final : public entity<0b1000>
 {
     direction() = default;
+
+    /// Create a normalized direction
     direction(float x, float y, float z) noexcept
     {
         parts[0].reg = _mm_set_ps(x, y, z, 0.f);
+        normalize();
     }
 
     // Provide conversion operator from parent class entity
@@ -39,14 +43,45 @@ struct direction final : public entity<0b1000>
         return parts[0].data[3];
     }
 
+    float& x() noexcept
+    {
+        return parts[0].data[3];
+    }
+
     float y() const noexcept
     {
         return parts[0].data[2];
     }
 
+    float& y() noexcept
+    {
+        return parts[0].data[2];
+    }
+
     float z() const noexcept
     {
         return parts[0].data[1];
     }
+
+    float& z() noexcept
+    {
+        return parts[0].data[1];
+    }
+
+    /// Normalize this direction by dividing all components by the square
+    /// magnitude
+    ///
+    /// !!! tip
+    ///
+    ///     Direction normalization divides the coordinates by the quantity
+    ///     a^2 + b^2 + c^2. This is done using the `rcpps` instruction with a
+    ///     maximum relative error of $1.5\times 2^{-12}$.
+    void normalize() noexcept
+    {
+        // Fast reciprocal operation to divide by a^2 + b^2 + c^2. The maximum
+        // relative error for the rcp approximation is 1.5*2^-12 (~.00036621)
+        __m128 tmp   = _mm_rcp_ps(_mm_dp_ps(p3(), p3(), 0b11101110));
+        parts[0].reg = _mm_mul_ps(parts[0].reg, tmp);
+    }
 };
-}
+} // namespace kln