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).
Zitieren
Lesezeichen