Angle Angle::subHelper(const Angle &a)
{
Angle temp = *this;
if (sign(temp.d) == sign(a.d))
{
temp.d -= a.d;
temp.m -= a.m;
temp.s -= a.s;
temp.t -= a.t;
}
else
{
temp.d = sign(temp.d) * (abs(temp.d) + abs(a.d));
temp.m += a.m;
temp.s += a.s;
temp.t += a.t;
}
temp.normalize();
return temp;
}
TEST(Angle, PositiveNormalize){
Angle a = M_PI * 5.5 * Angle::rad;
Angle b = a.normalize();
ASSERT_EQ(b.in_range(M_PI*1.5*Angle::rad), true);
}
TEST(Angle, NegativeNormalize){
Angle a = M_PI * -5.2 * Angle::rad;
Angle b = a.normalize();
ASSERT_EQ(b.in_range(M_PI*0.8*Angle::rad), true);
}
/**
* Performs a range query in the R-Tree, where the range is defined by the center +- the tolerances given.
* @param center The center of the query volume.
* @param xTolerance The absolute values to add and subtract to obtain the range in x dimension.
* @param yTolerance
* @param angleTolerance
* @return All points within the specified interval.
*/
vector<ContourIndex::ContourPointIdx> ContourIndex::symmetricRange(Contour::ContourPart center, float xTolerance, float yTolerance,
Angle angleTolerance, Angle curvatureTolerance
) {
assert(angleTolerance<=Angle(180,Angle::DEGREE)); // +-180˚ covers everything, more would cause duplicate results (because the generated regions will overlap)
assert(curvatureTolerance<=Angle(180,Angle::DEGREE));
int NORMAL_DIM = 2; // index of the normal orientation angle in the region bounds arrays
int CURVATURE_DIM = 3; // index of the curvature orientation angle in the region bounds arrays
double rLow[numDimensions], rHigh[numDimensions];
Angle lowerNormalBound = center->getAngularFeature(indexAngularFeature).normalize() - angleTolerance;
Angle upperNormalBound = center->getAngularFeature(indexAngularFeature).normalize() + angleTolerance;
Angle lowerCurvatureBound = center->curvature.normalize() - curvatureTolerance;
Angle upperCurvatureBound = center->curvature.normalize() + curvatureTolerance;
// location is unproblematic, no modulo here
rLow[0] = center->x - xTolerance;
rLow[1] = center->y - yTolerance;
rHigh[0] = center->x + xTolerance;
rHigh[1] = center->y + yTolerance;
// SPLIT RANGES FOR MODULO BEHAVIOR OF ANGLES
vector<SpatialIndex::Region> regions;
// central region
rLow[NORMAL_DIM] = std::max(0.f, lowerNormalBound.degree());
rHigh[NORMAL_DIM] = std::min(360.f, upperNormalBound.degree());
rLow[CURVATURE_DIM] = std::max(0.f, lowerCurvatureBound.degree());
rHigh[CURVATURE_DIM] = std::min(360.f, upperCurvatureBound.degree());
regions.push_back(SpatialIndex::Region(rLow, rHigh, numDimensions));
// normal orientation overshoot
if(upperNormalBound.degree() > 360){
// 0 to normalize(upperNormal) in normal dimension
rLow[NORMAL_DIM] = 0;
rHigh[NORMAL_DIM] = upperNormalBound.normalize().degree();
rLow[CURVATURE_DIM] = std::max(0.f, lowerCurvatureBound.degree()); // clip the rest
rHigh[CURVATURE_DIM] = std::min(360.f, upperCurvatureBound.degree());
regions.push_back(SpatialIndex::Region(rLow, rHigh, numDimensions));
}
// normal orientation undershoot
if(lowerNormalBound.degree() < 0){
// normalize(lowerNormal) to 360 in normal dimension
rLow[NORMAL_DIM] = lowerNormalBound.normalize().degree();
rHigh[NORMAL_DIM] = 360;
rLow[CURVATURE_DIM] = std::max(0.f, lowerCurvatureBound.degree()); // clip the rest
rHigh[CURVATURE_DIM] = std::min(360.f, upperCurvatureBound.degree());
regions.push_back(SpatialIndex::Region(rLow, rHigh, numDimensions));
}
// curvature orientation overshoot
if(upperCurvatureBound.degree() > 360){
// 0 to normalize(lowerCurvature) in curvature dimension
rLow[CURVATURE_DIM] = 0;
rHigh[CURVATURE_DIM] = upperCurvatureBound.normalize().degree();
rLow[NORMAL_DIM] = std::max(0.f, lowerNormalBound.degree()); // clip the rest
rHigh[NORMAL_DIM] = std::min(360.f, upperNormalBound.degree());
regions.push_back(SpatialIndex::Region(rLow, rHigh, numDimensions));
}
// curvature orientation undershoot
if(lowerCurvatureBound.degree() < 0){
// normalize(lowerCurvature) to 360 in curvature dimension
rLow[CURVATURE_DIM] = lowerCurvatureBound.normalize().degree();
rHigh[CURVATURE_DIM] = 360;
rLow[NORMAL_DIM] = std::max(0.f, lowerNormalBound.degree()); // clip the rest
rHigh[NORMAL_DIM] = std::min(360.f, upperNormalBound.degree());
regions.push_back(SpatialIndex::Region(rLow, rHigh, numDimensions));
}
// combined overshoot
if(upperNormalBound.degree() > 360 && upperCurvatureBound.degree() > 360){
// 0 to normalize(upperCurvature) in curvature dimension
// 0 to normalize(upperNormal) in normal dimension
rLow[NORMAL_DIM] = 0;
rHigh[NORMAL_DIM] = upperNormalBound.normalize().degree();
rLow[CURVATURE_DIM] = 0;
rHigh[CURVATURE_DIM] = upperCurvatureBound.normalize().degree();
regions.push_back(SpatialIndex::Region(rLow, rHigh, numDimensions));
}
// combined undershoot
if(lowerNormalBound.degree() < 0 && lowerCurvatureBound.degree() < 0){
// normalize(lowerNormal) to 360 in normal dimension
// normalize(lowerCurvature) to 360 in curvature dimension
rLow[NORMAL_DIM] = lowerNormalBound.normalize().degree();
rHigh[NORMAL_DIM] = 360;
rLow[CURVATURE_DIM] = lowerCurvatureBound.normalize().degree();
rHigh[CURVATURE_DIM] = 360;
regions.push_back(SpatialIndex::Region(rLow, rHigh, numDimensions));
}
// QUERY ALL REGIONS AND CONCAT RESULTS
vector<ContourPointIdx> *result = new vector<ContourPointIdx>();
for (std::vector<SpatialIndex::Region>::iterator r = regions.begin(); r != regions.end(); ++r) {
CollectIndicesVisitor cv(this);
tree->intersectsWithQuery(*r, cv);
for (vector<ContourPointIdx>::iterator it = cv.result.begin(); it != cv.result.end(); ++it)
result->push_back(*it);
}
return *result;
}