[Maths] Create a block for Atan2

[jabrena]

To develop the method GoTo(x,y), it is necessary to calculate this formula. https://en.wikipedia.org/wiki/Atan2

atan2(dY,dX) will give you the absolute heading of pendpend with respect to pstartpstart.

Four-quadrant inverse tangent. atan2(Y,X) returns the four-quadrant inverse tangent (tan-1) of Y and X, which must be real.

Create this block in the file odometry-maths.ev3 and import later in odometry.ev3 to be used in the GoTo(x,y) methods

Theory: http://www.eee.hku.hk/~msang/atan2 http://en.cppreference.com/w/cpp/numeric/math/atan2

[jabrena]

This link has a possible implementation: http://gamedev.stackexchange.com/questions/14602/what-are-atan-and-atan2-used-for-in-games

x = -2
y = 2
angle = calculateAngle(y, x);

double CalculateAngle(double y, double x)
{
    double angle = 0;
    if (x == 0)
    {
        if (y == 0)
            angle = 0;
        else if (y > 0)
            angle = Math.PI/2;
        else
            angle = -Math.PI/2;
    }
    else
    {
        angle = Math.Atan(y/x);
        if (x < 0)
        {
            if (y > 0)
            {
                angle += Math.PI;
            }
            else if (y < 0)
            {
                angle -= Math.PI;
            }
            else
            {
                angle = Math.PI;
            }
        }
    }
    return angle;
}

[jabrena]

This link is pretty interesting: http://edspi31415.blogspot.co.uk/2013/11/atan2-using-tan-1-and-anglearg-various.html

The function atan2(y,x) is defined as:

atan2(y,x) = tan^-1 (y/x) with respect to the quadrant the point (x, y) is in. In case you didn't know, with respect to point (x, y): 

(x, y) is in Quadrant I if x > 0 and y > 0
(x, y) is in Quadrant II if x < 0 and y > 0
(x, y) is in Quadrant III if x < 0 and y < 0
(x, y) is in Quadrant IV if x > 0 and y < 0

If the point is in quadrant I:
Use atan(y/x)

If the point is in quadrant II or III:
Use atan(y/x) + 180° in degrees mode
Use atan(y/x) + π in radians mode

If the point is in quadrant IV:
Use atan(y/x) + 360° in degrees mode
Use atan(y/x) + 2*π in radians mode

Special cases have to be used x or y is equal to 0:

If x=0 and y<0, the angle is 270° (3*π/2 radians)
If x=0 and y>0, the angle is 90° (π/2 radians)
If y=0 and x<0, the angle is 180° (π radians) 
If y=0 and x>0, the angle is 360° or 0° (2*π or 0 radians)

[jabrena]

https://en.wikipedia.org/wiki/Atan2#Definition

[jabrena]

https://en.wikipedia.org/wiki/Inverse_trigonometric_functions

[jabrena]

http://www.rapidtables.com/math/trigonometry/arctan.htm

[jabrena]

It is possible test results online: http://scriptun.com/php/online/atan2 https://www.medcalc.org/manual/atan2_function.php

[jabrena]

It is necessary to review tests. Creating the concept about Assert to automate Math tests

[jabrena]

It is necessary to test with the second definition of Atan2 in order to run all Automated Test:

[jabrena]

Case1:

a = y;
b = x;

2*atan(a/sqrt(b^2+a^2+b))

[jabrena]

http://opensource.apple.com//source/Libm/Libm-315/Source/Intel/atan2f.s

[jabrena]

https://en.wikipedia.org/wiki/CORDIC

[jabrena]

http://www.gamedev.net/topic/441464-manually-implementing-atan2-or-atan/

public double aTan2(double y, double x) {
    double coeff_1 = Math.PI / 4d;
    double coeff_2 = 3d * coeff_1;
    double abs_y = Math.abs(y);
    double angle;
    if (x >= 0d) {
        double r = (x - abs_y) / (x + abs_y);
        angle = coeff_1 - coeff_1 * r;
    } else {
        double r = (x + abs_y) / (abs_y - x);
        angle = coeff_2 - coeff_1 * r;
    }
    return y < 0d ? -angle : angle;
}

http://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization

//-----------------------------------------------
// Fast arctan2
float arctan2(float y, float x)
{
   coeff_1 = pi/4;
   coeff_2 = 3*coeff_1;
   abs_y = fabs(y)+1e-10      // kludge to prevent 0/0 condition
   if (x>=0)
   {
      r = (x - abs_y) / (x + abs_y);
      angle = coeff_1 - coeff_1 * r;
   }
   else
   {
      r = (x + abs_y) / (abs_y - x);
      angle = coeff_2 - coeff_1 * r;
   }
   if (y < 0)
   return(-angle);     // negate if in quad III or IV
   else
   return(angle);
}

[jabrena]

Testing Algorithm: Volkan Salma: https://gist.github.com/volkansalma/2972237

float atan2_approximation1(float y, float x)
{
    //http://pubs.opengroup.org/onlinepubs/009695399/functions/atan2.html
    //Volkan SALMA

    const float ONEQTR_PI = M_PI / 4.0;
    const float THRQTR_PI = 3.0 * M_PI / 4.0;
    float r, angle;
    float abs_y = fabs(y) + 1e-10f;      // kludge to prevent 0/0 condition
    if ( x < 0.0f )
    {
        r = (x + abs_y) / (abs_y - x);
        angle = THRQTR_PI;
    }
    else
    {
        r = (x - abs_y) / (x + abs_y);
        angle = ONEQTR_PI;
    }
    angle += (0.1963f * r * r - 0.9817f) * r;
    if ( y < 0.0f )
        return( -angle );     // negate if in quad III or IV
    else
        return( angle );

}

[jabrena]

The implementation based on Volkan Salma run, but it is not perfect. Next week, it is necessary to test other implentation

[jabrena]

At the moment, the Block Atan2, has 3 implementations:

Implementation1: Wikipedia, definition 1:

This implementation has some problems for some cases.

Implementation 2: Gamedev:

This implementation doesn´t work.

http://www.gamedev.net/topic/441464-manually-implementing-atan2-or-atan/ http://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization

Implementation 3: Approximation 1 from Volkan Salma:

https://gist.github.com/volkansalma/2972237

This implementation works but exist some cases that it is necessary to tune.

[jabrena]

Next week, I will implement the second case of Volkan: https://gist.github.com/volkansalma/2972237

this idea:

and this implementation: http://www.eee.hku.hk/~msang/atan2

[jabrena]

Other implementation:

http://math.stackexchange.com/questions/1098487/atan2-faster-approximation/1105038

a := min (|x|, |y|) / max (|x|, |y|)
s := a * a
r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
if |y| > |x| then r := 1.57079637 - r
if x < 0 then r := 3.14159274 - r
if y < 0 then r := -r

[jabrena]

http://www.cise.ufl.edu/~cop4600/cgi-bin/lxr/http/source.cgi/lib/math/atan2.c

[jabrena]

https://github.com/ducha-aiki/fast_atan2/blob/master/fast_atan.cpp