Ticket #3815: Length_approx.patch

source/maths/FixedVector2D.h

@@ -116 +116 @@
         return r;
     }
 
+    // Rough approximation of the length.
+    fixed Length_approx() const
+    {
+        // CORDIC algorithm: Rotate (x,y) onto the x-axis and use the new x as an approximation of the length.
+
+        // TODO: Choose an appropriate value of max_loop_var.
+        // Error: N := max_loop_var - 1, emax_rel = 1 - cos(arctan(1/2^N)) = 1 - 1/sqrt(1+4^(-N))
+        // In this case: Rel. error <= emax_rel (N=7) ~ 3*10^-5
+
+        const i32 max_loop_var = 8;
+        // Because we use multiples of rotation matrices, we need to scale the result.
+        // Factor = Product of (1+4^(-n))^(-1/2) from 0 to max_loop_var - 1.
+        // The factor below is (obviously) only right for max_loop_var = 8.
+        CFixed_15_16 corr_factor = CFixed_15_16::FromDouble(0.60725911229889273006029454182250359069);
+
+        i32 x = X.GetInternalValue();
+        i32 y = Y.GetInternalValue();
+        // We want x >= 0.
+        // TODO: Further optimizations.
+        if (x < 0)
+            x *= -1;
+
+        i32 tmp;
+        for (u8 loop_var = 0; loop_var < max_loop_var; ++loop_var)
+        {
+            tmp = x >> loop_var;
+            if(y < 0)
+            {
+                // (x,y) -> (x,y) + 1/2^n * (-y,x)
+                x -= y >> loop_var;
+                y += tmp;
+            }
+            else
+            {
+                // (x,y) -> (x,y) + 1/2^n * (y,-x)
+                x += y >> loop_var;
+                y -= tmp;
+            }
+        }
+        CFixed_15_16 result;
+        result.SetInternalValue(x);
+        return result.Multiply(corr_factor);
+    }
+
     /**
      * Returns -1, 0, +1 depending on whether length is less/equal/greater
      * than the argument.