/* Insert point into the Pyramid Tree.
* Returns true if the point was inserted successfully and
* false if the point is already stored in the structure. */
inline bool insert(const Point<D>& point)
{
// Retrieve containing bucket by hashing point into key
HashType searchKey = hashPoint(point);
// Search underlying 1D structure to find point's bucket
Bucket* bucket = NULL;
typename OneDMap::iterator it = hashMap.find(searchKey);
if (it != hashMap.end())
bucket = &(it->second);
if (bucket) // if bucket for point exists
{
// If point is stored in bucket - it ALREADY EXISTS
if (getPointIndexInBucket(point, bucket) != -1)
{
return false;
}
else
{
bucket->points.push_back(point);
bucket->pointSums.push_back(point.sum());
return true;
}
}
else // if bucket does not exist for point, create it!
{
Bucket newBucket;
newBucket.points.push_back(point);
newBucket.pointSums.push_back(point.sum());
hashMap[searchKey] = newBucket;
return true;
}
}
void Hash2D::hashPoints( std::vector<Point2D> & list)
{
for (std::vector<Point2D>::iterator it = list.begin(); it != list.end(); ++it)
{
hashPoint(*it);
}
}
inline Bucket* getContainingBucket(const Point<D>& point)
{
// Hash point into one-dimensional key
HashType searchKey = hashPoint(point);
// Search underlying structure to find point's bucket
typename OneDMap::iterator it = hashMap.find(searchKey);
if (it != hashMap.end())
return &(it->second); // pointer to bucket
else
return NULL;
}
Cache::i_Primitive CGALCache::hashPrimitive(CGALPrimitive* cg)
{
i_PointList pi;
foreach(CGAL::Point3 pt, cg->getCGALPoints())
pi.append(hashPoint(pt));
i_PolygonList pgi;
foreach(CGALPolygon* pg, cg->getCGALPolygons())
pgi.append(hashPolygon(pg));
return i_Primitive(pi,pgi);
}
Cache::i_Primitive Cache::hashPrimitive(Primitive* pr)
{
i_PointList pi;
foreach(Point pt, pr->getPoints())
pi.append(hashPoint(pt));
i_PolygonList pgi;
foreach(Polygon* pg, pr->getPolygons())
pgi.append(hashPolygon(pg));
return i_Primitive(pi,pgi);
}
void PathOrderOptimizer::optimize()
{
const float incommingPerpundicularNormalScale = 0.0001f;
std::map<uint32_t, std::vector<unsigned int>> location_to_polygon_map;
std::vector<bool> picked;
for(unsigned int i=0; i<polygons.size(); i++)
{
int best = -1;
float bestDist = 0xFFFFFFFFFFFFFFFFLL;
PolygonRef poly = polygons[i];
for(unsigned int j=0; j<poly.size(); j++)
{
float dist = vSize2f(poly[j] - startPoint);
if (dist < bestDist)
{
best = j;
bestDist = dist;
}
}
polyStart.push_back(best);
picked.push_back(false);
if (poly.size() == 2)
{
Point p0 = poly[0];
Point p1 = poly[1];
location_to_polygon_map[hashPoint(p0)].push_back(i);
location_to_polygon_map[hashPoint(p1)].push_back(i);
}
}
Point incommingPerpundicularNormal(0, 0);
Point p0 = startPoint;
for(unsigned int n=0; n<polygons.size(); n++)
{
int best = -1;
float bestDist = 0xFFFFFFFFFFFFFFFFLL;
for(unsigned int i : location_to_polygon_map[hashPoint(p0)])
{
if (picked[i] || polygons[i].size() < 1)
continue;
float dist = vSize2f(polygons[i][0] - p0);
dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][1] - polygons[i][0], 1000))) * incommingPerpundicularNormalScale;
if (dist < bestDist)
{
best = i;
bestDist = dist;
polyStart[i] = 0;
}
dist = vSize2f(polygons[i][1] - p0);
dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][0] - polygons[i][1], 1000))) * incommingPerpundicularNormalScale;
if (dist < bestDist)
{
best = i;
bestDist = dist;
polyStart[i] = 1;
}
}
if (best == -1)
{
for(unsigned int i=0; i<polygons.size(); i++)
{
if (picked[i] || polygons[i].size() < 1)
continue;
if (polygons[i].size() == 2)
{
float dist = vSize2f(polygons[i][0] - p0);
dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][1] - polygons[i][0], 1000))) * incommingPerpundicularNormalScale;
if (dist < bestDist)
{
best = i;
bestDist = dist;
polyStart[i] = 0;
}
dist = vSize2f(polygons[i][1] - p0);
dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][0] - polygons[i][1], 1000))) * incommingPerpundicularNormalScale;
if (dist < bestDist)
{
best = i;
bestDist = dist;
polyStart[i] = 1;
}
} else {
float dist = vSize2f(polygons[i][polyStart[i]] - p0);
if (dist < bestDist)
{
best = i;
bestDist = dist;
}
}
}
}
if (best > -1)
{
if (polygons[best].size() == 2)
{
int endIdx = (polyStart[best] + 1) % 2;
p0 = polygons[best][endIdx];
incommingPerpundicularNormal = crossZ(normal(polygons[best][endIdx] - polygons[best][polyStart[best]], 1000));
} else {
p0 = polygons[best][polyStart[best]];
incommingPerpundicularNormal = Point(0, 0);
}
picked[best] = true;
polyOrder.push_back(best);
}
}
p0 = startPoint;
for(int nr : polyOrder)
{
PolygonRef poly = polygons[nr];
if (poly.size() > 2)
{
int best = -1;
float bestDist = 0xFFFFFFFFFFFFFFFFLL;
bool orientation = poly.orientation();
for(unsigned int i=0; i<poly.size(); i++)
{
const int64_t dot_score_scale = 2000;
float dist = vSize2f(polygons[nr][i] - p0);
Point n0 = normal(poly[(i+poly.size()-1)%poly.size()] - poly[i], dot_score_scale);
Point n1 = normal(poly[i] - poly[(i + 1) % poly.size()], dot_score_scale);
float dot_score = dot(n0, n1) - dot(crossZ(n0), n1);
if (orientation)
dot_score = -dot_score;
dist += dot_score;
if (dist < bestDist)
{
best = i;
bestDist = dist;
}
}
polyStart[nr] = best;
}
if (poly.size() <= 2)
{
p0 = poly[(polyStart[nr] + 1) % 2];
} else {
p0 = poly[polyStart[nr]];
}
}
}
