void Resample(PointVector& points, size_t n)
{
double I = PathLength(points) / (n - 1); // interval length
double D = 0.0;
PointVector newpoints;
newpoints.push_back(points[0]);
for (size_t i = 1; i < points.size(); ++i)
{
double d = Distance(points[i - 1], points[i]);
if ((D + d) >= I)
{
double qx = points[i - 1]->X + ((I - D) / d) * (points[i]->X - points[i - 1]->X);
double qy = points[i - 1]->Y + ((I - D) / d) * (points[i]->Y - points[i - 1]->Y);
PointPtr q(new Point(qx, qy));
newpoints.push_back(q);
points.insert(points.begin()+i,q);
D = 0.0;
}
else D += d;
}
// somtimes we fall a rounding-error short of adding the last point, so add it if so
if (newpoints.size() == n - 1)
{
newpoints.push_back(points[points.size() - 1]);
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
void ScaleToSquare(PointVector& points, double size)
{
RectanglePtr B = BoundingBox(points);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = points[i]->X * (size / B->Width);
double qy = points[i]->Y * (size / B->Height);
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
void TranslateToOrigin(PointVector& points)
{
PointPtr c = Centroid(points);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = points[i]->X - c->X;
double qy = points[i]->Y - c->Y;
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
PointPairVector Space::pairsInZone(const PointVector& zone, unsigned int d)
{
PointPairVector result;
PointVector::const_iterator iter = zone.begin();
PointVector::const_iterator endIter = zone.end();
for(;iter!=endIter;++iter)
{
for(unsigned int i = 1;(iter+i)!=endIter;++i)
{
// optimization
if(iter->distanceYTo(*(iter+i))>d) // if distance by Y is higher than d
{
if(iter->y < (iter+i)->y) // if current point is before second one
break; // there won't be more interesting points in d-neighborhood
else
continue; // we could not reach interesting points yet
}
if(iter->distanceTo(*(iter+i))<=d) // if distance is lower than d
{
result.insert(PointPair(*iter,*(iter+i))); // add pair of d-neighbors
}
}
}
return result;
}
void RotateBy(PointVector& points, double theta)
{
PointPtr c = Centroid(points);
double cosine = cos(theta);
double sine = sin(theta);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = (points[i]->X - c->X) * cosine - (points[i]->Y - c->Y) * sine + c->X;
double qy = (points[i]->X - c->X) * sine + (points[i]->Y - c->Y) * cosine + c->Y;
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
PointPairVector Space::bruteNeighbors(const PointVector& vec, unsigned int start, unsigned int end, unsigned int d) const
{
PointPairVector result;
PointVector::const_iterator i = vec.begin()+start;
PointVector::const_iterator j = vec.begin()+start+1;
PointVector::const_iterator endIter = (end>=vec.size()) ? vec.end() : vec.begin()+end+1;
for(;i<endIter;++i)
{
for(j=i+1;j<endIter;++j)
{
if(j->distanceTo(*i)<=d)
{
result.insert(PointPair(*i,*j));
}
}
}
return result;
}
void util::printPointVector(const PointVector pVect, int fieldWidth)
{
PointVector::const_iterator point;
for (point = pVect.begin(); point != pVect.end(); ++point) {
std::cout << "["
<< std::setw(fieldWidth) << (*point).x << " , "
<< std::setw(fieldWidth) << (*point).y << "] ";
}
std::cout << std::endl;
}
PointVector Space::leaveFromZone(const PointVector& vec, const Point& splitPoint, unsigned int d)
{
PointVector result;
PointVector::const_iterator iter = vec.begin();
PointVector::const_iterator endIter = vec.end();
for(;iter!=endIter;++iter)
{
if(iter->distanceXTo(splitPoint)<=d)
result.push_back(*iter);
}
return result;
}
PointVector ImageRegistrator::removeBadPoints(PointVector points, std::vector<uchar> status)
{
std::vector<uchar>::iterator statusIter = status.begin();
PointVector::iterator pointIter = points.begin();
PointVector goodPoints(points.size() - std::count(status.begin(), status.end(), 0));
for (int i = 0; statusIter != status.end(); ++statusIter, ++pointIter) {
if (*statusIter == 1) {
goodPoints[i] = *pointIter;
++i;
}
}
return goodPoints;
}
bool testIterator()
{
PointVector<25,int> aPoint;
PointVector<4, int> avector;
trace.beginBlock("Point Iterator Test");
for (unsigned int i=0;i<25;++i)
aPoint.at(i) = i;
trace.info() << "aPoint="<<aPoint<< std::endl;
trace.info() << "With iterator: ";
for (PointVector<25,int>::Iterator it = aPoint.begin() ; it != aPoint.end(); ++it)
trace.info() << (*it) <<" " ;
trace.info() << std::endl;
trace.endBlock();
return true;
}
