/**
* Insert a new vertex.
* @param pt A new point for the shape
*/
void ConvexPolygon::insert(const V2D &pt) {
m_vertices.push_back(pt);
// Update extrema
m_minX = std::min(m_minX, pt.X());
m_maxX = std::max(m_maxX, pt.X());
m_minY = std::min(m_minY, pt.Y());
m_maxY = std::max(m_maxY, pt.Y());
}
/**
* Angle between this and another vector
*/
double V2D::angle(const V2D &other) const {
double ratio = this->scalar_prod(other) / (this->norm() * other.norm());
if (ratio >= 1.0) // NOTE: Due to rounding errors, if "other"
return 0.0; // is nearly the same as "this" or
else if (ratio <= -1.0) // as "-this", ratio can be slightly
return M_PI; // more than 1 in absolute value.
// That causes acos() to return NaN.
return acos(ratio);
}
void
BlackBot::wallCollision(const std::vector<FWEdge::EID> &eids, const V2D &cv)
{
/* std::cerr << "I am "<<pid<<" and collided with wall(s):";
for (unsigned w=0;w<eids.size();++w){
std::cerr << " "<<eids[w];
}
std::cerr << "\n";
*/
dir=vdir(cv.rot(M_PI/180*(threshold+90)));
followMode=false;
}
/**
* Classify a point with respect to an edge, i.e. left, right of edge etc.
* @param pt :: A point as V2D
* @param edge :: A test edge object
* @returns The type of the point
*/
PointClassification classify(const V2D &pt, const PolygonEdge &edge) {
V2D p2 = pt;
const V2D &a = edge.direction();
V2D b = p2 - edge.start();
double sa = a.X() * b.Y() - b.X() * a.Y();
if (sa > 0.0) {
return OnLeft;
}
if (sa < 0.0) {
return OnRight;
}
if ((a.X() * b.X() < 0.0) || (a.Y() * b.Y() < 0.0)) {
return Behind;
}
if (a.norm() < b.norm()) {
return Beyond;
}
if (edge.start() == p2) {
return Origin;
}
if (edge.end() == p2) {
return Destination;
}
return Between;
}
vector<pair<int, int> > getMatch(V2D vX, V2D vY) {
X = vX;
Y = vY;
SZ = X.size();
for(int i = 0; i < SZ; ++i)
for(int j = 0; j < SZ; ++j)
Y[i][vY[i][j]] = j;
mX.assign(SZ, -1);
mY.assign(SZ, -1);
k.assign(SZ, 0);
bool yes = true;
while(true) {
bool mark = true;
int i;
for(i = 0; i < SZ; ++i) {
if(mX[i] == -1) {
while(k[i] < SZ) {
int j = X[i][k[i]];
if( mY[j] == -1 ) {
mX[i] = j;
mY[j] = i;
break;
}
if( Y[j][i] < Y[j][mY[j]] ) {
mX[mY[j]] = -1;
k[mY[j]]++;
mX[i] = j;
mY[j] = i;
break;
}
++k[i];
}
if(mX[i] == -1)
mark = false;
break;
}
}
if(i == N)
break;
if(!mark) {
yes = false;
break;
}
}
vector<pair<int, int> > ans(0);
if(!yes) return ans;
for(int i = 0; i < SZ; ++i)
ans.push_back(make_pair(i, mX[i]));
return ans;
}
R Line::dist(const V2D &p, V2D &vn)
{
R r = project(p);
R l = 0;
if ((r>=0) && (r<=m_l))
{
l = (p-m_a)*m_n;
if (l<0)
{
l *= -1;
vn = m_n*((R) -1);
}
else vn = m_n;
}
else
{
V2D va = (p-m_a);
V2D vb = (p-m_b);
R a = va.norm2sqr();
R b = vb.norm2sqr();
if (a < b)
{
vn = va;
l = sqrt(a);
}
else
{
vn = vb;
l = sqrt(b);
}
vn = vn * (1/l);
}
return l;
}
/**
* Compute the area of a triangle given by 3 points (a,b,c) using the
* convention in "Computational Geometry in C" by J. O'Rourke
* @param a :: The first vertex in the set
* @param b :: The second vertex in the set
* @param c :: The third vertex in the set
*/
double ConvexPolygon::triangleArea(const V2D &a, const V2D &b,
const V2D &c) const {
return 0.5 * (b.X() - a.X()) * (c.Y() - a.Y()) -
(c.X() - a.X()) * (b.Y() - a.Y());
}
