#include <math.h>                   /* acos(), M_PI */

#include "Polygon.h"

#define FP_EQUAL(x,y) (fabs((x)-(y)) < 1E-6)

/*
 * from http://local.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/
 * Return the angle between two vectors on a plane
 * The angle is from vector 1 to vector 2, positive anticlockwise
 * The result is between -pi -> pi
*/
double angle2D(double x1, double y1, double x2, double y2)
{
    double dtheta, theta1, theta2;

    theta1 = atan2(y1,x1);
    theta2 = atan2(y2,x2);
    dtheta = theta2 - theta1;
    while (dtheta > M_PI)
        dtheta -= 2.0 * M_PI;
    while (dtheta < -M_PI)
        dtheta += 2.0 * M_PI;

    return dtheta;
}

bool Polygon::containsPoint2D(const Vector & v) const
{
    double angle_sum = 0.0;
    for (int i = 0, sz = size(); i < sz; i++)
    {
        Vector v1 = *(*this)[i] - v;
        Vector v2 = *(*this)[(i+1) % sz] - v;
        double angle = angle2D(v1[0], v1[1], v2[0], v2[1]);
        angle_sum += angle;
    }
    return FP_EQUAL(angle_sum, 2.0 * M_PI);
}

bool Polygon::containsPointConvex(const Vector & v) const
{
    double angle_sum = 0.0;
    for (int i = 0, sz = size(); i < sz; i++)
    {
        Vector v1 = *(*this)[i] - v;
        Vector v2 = *(*this)[(i+1) % sz] - v;
        double cosine = (v1 % v2) / (v1.mag() * v2.mag());
        double angle = acos(cosine);
        angle_sum += angle;
    }
    return FP_EQUAL(angle_sum, 2.0 * M_PI);
}