Jepp, die vier Quadranten fehlen:
Vielleicht hilft Dir der folgende Code


Code:
double AngleOf(double x, double y)
{
    if ((x == 0) && (y == 0))
        return 0;

    double rval = 0;
    uint8_t iPre;
    if (y >= 0) //Quadrant determination
        iPre = (x > 0 ? 0 : 1); 
    else
        iPre = (x > 0 ? 3 : 2);

    if (x != 0)
    {
        switch (iPre)
        {
            case 0: //1st Quadrant
                rval = atan(y / x);
                break;
            case 1: //2nd Quadrant
                rval = M_PI + atan(y / x);
                break;
            case 2: //3rd Quadrant
                rval = atan(y / x) - M_PI;
                break;
            case 3: //4th Quadrant
                rval = atan(y / x);
                break;
        }
    }

    else //special case: vertical lines 
    {
        if (y > 0)
            rval = M_PI / 2;
        else
            rval = -M_PI / 2;

    }
    return rval;
}
Auch noch ganz wichtig, damit der Winkel statt -270° dann später + 90° wird:
Code:
double AngleNorm(double angle)
{
    while (angle > M_PI)
        angle -= (M_PI * 2);
    while (angle <= -M_PI)
        angle += (M_PI * 2);

    return angle;
}

Wenn man sich jetzt noch die Mühe macht, die ganze Posiergeschichte in Datentypen zu fassen...

Code:
    typedef struct
	{
        double X;
        double Y;
    } Point_t;


    typedef struct
	{
        Point_t Point;
        double Orientation;
    } Pose_t;

    typedef struct
    {
        double Distance;
        double Angle;   
    } PoseChangeVector_t;
.... gibt die Rechnerei dann auch gleich die Distanz und den Winkel aus....

Code:
void GetChangeVector(Pose_t* actual, Point_t* target, PoseChangeVector_t* result)
{
    double dx = target->X - actual->Point.X;
    double dy = target->Y - actual->Point.Y;
     
    result->Distance = sqrt((dx*dx) + (dy*dy));
    result->Angle = AngleNorm ( AngleOf(dx, dy) - actual->Orientation);
}
...und Du kannst schon im Simulator (falls vorhanden)...

Code:
double GetChangeVectorTest()
{
    Pose_t actual;
    actual.Orientation = M_PI/4;
    actual.Point.X = 100;
    actual.Point.Y = 100;

    Point_t target;
    target.X = -100;
    target.Y = -100;

    PoseChangeVector_t change;

    GetChangeVector(&actual, &target, &change);
    return change.Angle;
}

... durch die Variation von actual- und target-Parametern Dein Ergebnis testen.

(Rennt bei mir seit drei Jahren, hab ich mir damals aber auch erst 'ne Stunde das Hirn verrenkt).