爱魔域

struct Point
{
float x,y;
};

float vectorMag(const Point &start, const Point &end)
{
return sqrtf((start.x - end.x) * (start.x - end.x) +
(start.y - end.y) * (start.y - end.y));
}

float DotProduct(const Point &cen, const Point &first, const Point &second)
{
dx1 = first.x - cen.x;
dx2 = second.x - cen.x;
dy1 = first.y - cen.y;
dy2 = second.y - cen.y;

return dx1 * dx2 + dy1 * dy2;
}

//假定逆时针为正向
bool SignByCrossProduct(const Point &cen, const Point &first, const Point &second)
{
dx1 = first.x - cen.x;
dx2 = second.x - cen.x;
dy1 = first.y - cen.y;
dy2 = second.y - cen.y;

if(dx1 * dy2 - dy1 * dx2 > 0) return true;
return false;
}

//夹角范围[-PI, PI]
float Angle(Point cen,Point first,Point second)
{
float dx1,dx2,dy1,dy2;
float angle;

float mag = vectorMag(cen, first) * vectorMag(cen, second);
angle = (float)acos(DotProduct(cen, first, second) / mag);

bool sign = SignByCrossProduct(cen, first, second);

if (!sign) angle = -angle;

return angle;
}