void insert(rbtree **root, rbtree *z) { current+=1; if (*root == NULL) { *root = z; } rbtree *n = *root; while (1) { int comparisonResult = n->T-z->T; if (comparisonResult == 0) { return; } else if (comparisonResult < 0) { if (leftOf(n) == NULL) { n->left=z; adjustAfterInsertion(*root, z); break; } n = leftOf(n); } else { // comparisonResult > 0 if (rightOf(n) == NULL) { n->right=z; adjustAfterInsertion(*root,z); break; } n = rightOf(n); } } }
bool KdTree::queryVisibilityRecursive(const Vector2& q1, const Vector2& q2, float radius, const ObstacleTreeNode* node) const { if (node == 0) { return true; } else { const Obstacle* const obstacle1 = node->obstacle; const Obstacle* const obstacle2 = obstacle1->nextObstacle; const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right)); } else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left)); } else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { /* One can see through obstacle from left to right. */ return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right); } else { const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); const float invLengthQ = 1.0f / absSq(q2 - q1); return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right)); } } }
static int edgesIntersect(Point * P, Point * Q, int n, int m) { int a = 0; int b = 0; int aa = 0; int ba = 0; int a1, b1; Point A, B; double cross; int bHA, aHB; Point p; int inflag = Unknown; /* int i = 0; */ /* int Reset = 0; */ do { a1 = (a + n - 1) % n; b1 = (b + m - 1) % m; subpt(&A, P[a], P[a1]); subpt(&B, Q[b], Q[b1]); cross = area_2(origin, A, B); bHA = leftOf(P[a1], P[a], Q[b]); aHB = leftOf(Q[b1], Q[b], P[a]); /* If A & B intersect, update inflag. */ if (intersection(P[a1], P[a], Q[b1], Q[b], &p)) return 1; /* Advance rules. */ if ((cross == 0) && !bHA && !aHB) { if (inflag == Pin) advance(b, ba, m); else advance(a, aa, n); } else if (cross >= 0) if (bHA) advance(a, aa, n); else { advance(b, ba, m); } else { /* if ( cross < 0 ) */ if (aHB) advance(b, ba, m); else advance(a, aa, n); } } while (((aa < n) || (ba < m)) && (aa < 2 * n) && (ba < 2 * m)); return 0; }
bool Line::operator &&(const Line &l2) const { // only for closed polys! int n = size()-1, m = l2.size()-1; int a = 0; int b = 0; int aa = 0; int ba = 0; int a1, b1; Coord A, B; double cross; bool bHA, aHB; int inflag = Unknown; do { a1 = (a + n - 1) % n; b1 = (b + m - 1) % m; A = at(a) - at(a1); B = l2[b] - l2[b1]; cross = tri_area_2(Coord(0,0), A, B ); bHA = leftOf( at(a1), at(a), l2[b] ); aHB = leftOf( l2[b1], l2[b], at(a) ); /* If A & B intersect, update inflag. */ if (intersection( at(a1), at(a), l2[b1], l2[b]).valid ) return 1; /* Advance rules. */ if ( (cross == 0) && !bHA && !aHB ) { if ( inflag == Pin ) advance( b, ba, m); else advance( a, aa, n); } else if ( cross >= 0 ) if ( bHA ) advance( a, aa, n); else { advance( b, ba, m); } else /* if ( cross < 0 ) */{ if ( aHB ) advance( b, ba, m); else advance( a, aa, n); } } while ( ((aa < n) || (ba < m)) && (aa < 2*n) && (ba < 2*m) ); return 0; }
sf::IntRect& fitRectByShrinking(sf::IntRect& innerRect, const sf::IntRect& outerRect) { int iLeft = leftOf(innerRect); int iRight = rightOf(innerRect); int iTop = topOf(innerRect); int iBottom = bottomOf(innerRect); int oLeft = leftOf(outerRect); int oRight = rightOf(outerRect); int oTop = topOf(outerRect); int oBottom = bottomOf(outerRect); iLeft = iLeft < oLeft ? oLeft : iLeft; iRight = iRight > oRight ? oRight : iRight; iTop = iTop < oTop ? oTop : iTop; iBottom = iBottom > oBottom ? oBottom : iBottom; return setRectByCorners(innerRect, iLeft, iTop, iRight, iBottom); }
void KdTree::queryObstacleTreeRecursive(Agent* agent, float rangeSq, const ObstacleTreeNode* node) const { if (node == 0) { return; } else { const Obstacle* const obstacle1 = node->obstacle; const Obstacle* const obstacle2 = obstacle1->nextObstacle; const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_); queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); if (distSqLine < rangeSq) { if (agentLeftOfLine < 0.0f) { /* * Try obstacle at this node only if agent is on right side of * obstacle (and can see obstacle). */ agent->insertObstacleNeighbor(node->obstacle, rangeSq); } /* Try other side of line. */ queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); } } }
void ObstacleKDTree::queryTreeRecursive( ProximityQuery *filter, Vector2 pt, float& rangeSq, const ObstacleTreeNode* node) const { if (node == 0) { return; } else { const Obstacle* const obstacle1 = node->_obstacle; const Vector2 P0 = obstacle1->getP0(); const Vector2 P1 = obstacle1->getP1(); const float agentLeftOfLine = leftOf( P0, P1, pt); queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_left : node->_right)); const float distSqLine = sqr(agentLeftOfLine) / absSq(P1 - P0); if (distSqLine < rangeSq) { if ( obstacle1->_doubleSided || agentLeftOfLine < 0.0f) { /* * Try obstacle at this node only if agent is on right side of * obstacle (and can see obstacle). */ float distSq = distSqPointLineSegment(node->_obstacle->getP0(), node->_obstacle->getP1(), pt); filter->filterObstacle(node->_obstacle, distSq); rangeSq = filter->getMaxObstacleRange(); } /* Try other side of line. */ queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_right : node->_left)); } } }
size_t RVOSimulator::addObstacle(const std::vector<Vector2>& vertices) { if (vertices.size() < 2) { return RVO_ERROR; } size_t obstacleNo = obstacles_.size(); for (size_t i = 0; i < vertices.size(); ++i) { Obstacle* obstacle = new Obstacle(); obstacle->point_ = vertices[i]; if (i != 0) { obstacle->prevObstacle = obstacles_.back(); obstacle->prevObstacle->nextObstacle = obstacle; } if (i == vertices.size() - 1) { obstacle->nextObstacle = obstacles_[obstacleNo]; obstacle->nextObstacle->prevObstacle = obstacle; } obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i+1)] - vertices[i]); if (vertices.size() == 2) { obstacle->isConvex_ = true; } else { obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i-1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i+1)]) >= 0); } obstacle->id_ = obstacles_.size(); obstacles_.push_back(obstacle); } return obstacleNo; }
Edge *Subdivision::locate(const Vec2& x, Edge *start) { Edge *e = start; double t = triArea(x, e->Dest(), e->Org()); if (t>0) { // x is to the right of edge e t = -t; e = e->Sym(); } while (true) { Edge *eo = e->Onext(); Edge *ed = e->Dprev(); double to = triArea(x, eo->Dest(), eo->Org()); double td = triArea(x, ed->Dest(), ed->Org()); if (td>0) // x is below ed if (to>0 || to==0 && t==0) {// x is interior, or origin endpoint startingEdge = e; return e; } else { // x is below ed, below eo t = to; e = eo; } else // x is on or above ed if (to>0) // x is above eo if (td==0 && t==0) { // x is destination endpoint startingEdge = e; return e; } else { // x is on or above ed and above eo t = td; e = ed; } else // x is on or below eo if (t==0 && !leftOf(eo->Dest(), e)) // x on e but subdiv. is to right e = e->Sym(); else if (rand()&1) { // x is on or above ed and t = to; // on or below eo; step randomly e = eo; } else { t = td; e = ed; } } }
void Reg::ConvexHull() { cerr << "Calculating Convex Hull Start\n"; vector<Pt> lt = getPoints(); std::sort(lt.begin(), lt.end()); lt[0].angle = -1.0; for (unsigned int a = 1; a < lt.size(); a++) { lt[a].calcAngle(lt[0]); } std::sort(lt.begin(), lt.end(), sortAngle); vector<Pt> uh = vector<Pt > (); uh.push_back(lt[0]); uh.push_back(lt[1]); for (int a = 2; a < (int) lt.size();) { cerr << "List len: " << uh.size() << "\n"; assert(uh.size() >= 2); Pt point1 = uh[uh.size() - 1]; Pt point2 = uh[uh.size() - 2]; Pt next = lt[a]; cerr << "P1: " << point1.ToString() << "P2: " << point2.ToString() << "Nx: " << next.ToString() << "\n"; if (leftOf(point2, point1, next)) { uh.push_back(next); a++; } else { uh.pop_back(); } } for (unsigned int i = 0; i < uh.size(); i++) { Pt p1 = uh[i]; Pt p2 = uh[(i + 1) % uh.size()]; Seg s = Seg(p1.x, p1.y, p2.x, p2.y); convexhull.push_back(s); } // convexhull = sortSegs(convexhull); cerr << "Calculating Convex Hull End\n"; }
// Helper: do we know that segment a is in front of b? // Implementation not anti-symmetric (that is to say, // _segment_in_front_of(a, b) != (!_segment_in_front_of(b, a)). // Also note that it only has to work in a restricted set of cases // in the visibility algorithm; I don't think it handles all // cases. See http://www.redblobgames.com/articles/visibility/segment-sorting.html bool _segment_in_front_of(const Wall* a, const Wall* b, const Vector_* relativeTo) { // NOTE: we slightly shorten the segments so that // intersections of the endpoints (common) don't count as // intersections in this algorithm Vector_ temp; bool A1 = leftOf(a, interpolate(&temp, &b->start, &b->end, 0.01)); bool A2 = leftOf(a, interpolate(&temp, &b->end, &b->start, 0.01)); bool A3 = leftOf(a, relativeTo); bool B1 = leftOf(b, interpolate(&temp, &a->start, &a->end, 0.01)); bool B2 = leftOf(b, interpolate(&temp, &a->end, &a->start, 0.01)); bool B3 = leftOf(b, relativeTo); // NOTE: this algorithm is probably worthy of a short article // but for now, draw it on paper to see how it works. Consider // the line A1-A2. If both B1 and B2 are on one side and // relativeTo is on the other side, then A is in between the // viewer and B. We can do the same with B1-B2: if A1 and A2 // are on one side, and relativeTo is on the other side, then // B is in between the viewer and A. if (B1 == B2 && B2 != B3) return true; if (A1 == A2 && A2 == A3) return true; if (A1 == A2 && A2 != A3) return false; if (B1 == B2 && B2 == B3) return false; // If A1 != A2 and B1 != B2 then we have an intersection. // Expose it for the GUI to show a message. A more robust // implementation would split segments at intersections so // that part of the segment is in front and part is behind. //demo_intersectionsDetected.push([a.p1, a.p2, b.p1, b.p2]); return false; // NOTE: previous implementation was a.d < b.d. That's simpler // but trouble when the segments are of dissimilar sizes. If // you're on a grid and the segments are similarly sized, then // using distance will be a simpler and faster implementation. }
KdTree::ObstacleTreeNode* KdTree::buildObstacleTreeRecursive(const std::vector<Obstacle*>& obstacles) { if (obstacles.empty()) { return 0; } else { ObstacleTreeNode* const node = new ObstacleTreeNode; size_t optimalSplit = 0; size_t minLeft = obstacles.size(); size_t minRight = obstacles.size(); for (size_t i = 0; i < obstacles.size(); ++i) { size_t leftSize = 0; size_t rightSize = 0; const Obstacle* const obstacleI1 = obstacles[i]; const Obstacle* const obstacleI2 = obstacleI1->nextObstacle; /* Compute optimal split node. */ for (size_t j = 0; j < obstacles.size(); ++j) { if (i == j) { continue; } const Obstacle* const obstacleJ1 = obstacles[j]; const Obstacle* const obstacleJ2 = obstacleJ1->nextObstacle; const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { ++leftSize; } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { ++rightSize; } else { ++leftSize; ++rightSize; } if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { break; } } if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { minLeft = leftSize; minRight = rightSize; optimalSplit = i; } } /* Build split node. */ std::vector<Obstacle*> leftObstacles(minLeft); std::vector<Obstacle*> rightObstacles(minRight); size_t leftCounter = 0; size_t rightCounter = 0; const size_t i = optimalSplit; const Obstacle* const obstacleI1 = obstacles[i]; const Obstacle* const obstacleI2 = obstacleI1->nextObstacle; for (size_t j = 0; j < obstacles.size(); ++j) { if (i == j) { continue; } Obstacle* const obstacleJ1 = obstacles[j]; Obstacle* const obstacleJ2 = obstacleJ1->nextObstacle; const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { leftObstacles[leftCounter++] = obstacles[j]; } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { rightObstacles[rightCounter++] = obstacles[j]; } else { /* Split obstacle j. */ const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_); const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); Obstacle* const newObstacle = new Obstacle(); newObstacle->point_ = splitpoint; newObstacle->prevObstacle = obstacleJ1; newObstacle->nextObstacle = obstacleJ2; newObstacle->isConvex_ = true; newObstacle->unitDir_ = obstacleJ1->unitDir_; newObstacle->id_ = sim_->obstacles_.size(); sim_->obstacles_.push_back(newObstacle); obstacleJ1->nextObstacle = newObstacle; obstacleJ2->prevObstacle = newObstacle; if (j1LeftOfI > 0.0f) { leftObstacles[leftCounter++] = obstacleJ1; rightObstacles[rightCounter++] = newObstacle; } else { rightObstacles[rightCounter++] = obstacleJ1; leftObstacles[leftCounter++] = newObstacle; } } } node->obstacle = obstacleI1; node->left = buildObstacleTreeRecursive(leftObstacles); node->right = buildObstacleTreeRecursive(rightObstacles); return node; } }
ObstacleTreeNode* ObstacleKDTree::buildTreeRecursive( const std::vector<Obstacle*>& obstacles) { if ( obstacles.empty() ) { return 0x0; } else { ObstacleTreeNode* const node = new ObstacleTreeNode; size_t optimalSplit = 0; size_t minLeft = obstacles.size(); size_t minRight = obstacles.size(); for (size_t i = 0; i < obstacles.size(); ++i) { size_t leftSize = 0; size_t rightSize = 0; const Obstacle* const obstacleI = obstacles[i]; const Vector2 I0 = obstacleI->getP0(); const Vector2 I1 = obstacleI->getP1(); /* Compute optimal split node. */ for (size_t j = 0; j < obstacles.size(); ++j) { if (i == j) { continue; } const Obstacle* const obstacleJ = obstacles[j]; const Vector2 J0 = obstacleJ->getP0(); const Vector2 J1 = obstacleJ->getP1(); const float j1LeftOfI = leftOf( I0, I1, J0 ); const float j2LeftOfI = leftOf( I0, I1, J1 ); if (j1LeftOfI >= -EPS && j2LeftOfI >= -EPS) { ++leftSize; } else if (j1LeftOfI <= EPS && j2LeftOfI <= EPS ) { ++rightSize; } else { ++leftSize; ++rightSize; } if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { break; } } if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { minLeft = leftSize; minRight = rightSize; optimalSplit = i; } } /* Build split node. */ std::vector<Obstacle*> leftObstacles(minLeft); std::vector<Obstacle*> rightObstacles(minRight); size_t leftCounter = 0; size_t rightCounter = 0; const size_t i = optimalSplit; const Obstacle* const obstacleI = obstacles[i]; const Vector2 I0 = obstacleI->getP0(); const Vector2 I1 = obstacleI->getP1(); for (size_t j = 0; j < obstacles.size(); ++j) { if (i == j) { continue; } Obstacle* const obstacleJ = obstacles[j]; const Vector2 J0 = obstacleJ->getP0(); const Vector2 J1 = obstacleJ->getP1(); const float j1LeftOfI = leftOf( I0, I1, J0 ); const float j2LeftOfI = leftOf( I0, I1, J1 ); if (j1LeftOfI >= -EPS && j2LeftOfI >= -EPS ) { leftObstacles[leftCounter++] = obstacles[j]; } else if (j1LeftOfI <= EPS && j2LeftOfI <= EPS ) { rightObstacles[rightCounter++] = obstacles[j]; } else { /* Split obstacle j. */ const float t = det( I1 - I0, J0 - I0 ) / det( I1 -I0, J0 - J1 ); const Vector2 splitpoint = J0 + t * ( J1 - J0 ); Obstacle* const newObstacle = new Obstacle(); newObstacle->_point = splitpoint; newObstacle->_prevObstacle = obstacleJ; newObstacle->_nextObstacle = obstacleJ->_nextObstacle; if ( newObstacle->_nextObstacle ) { obstacleJ->_nextObstacle = newObstacle; } newObstacle->_isConvex = true; newObstacle->_unitDir = obstacleJ->_unitDir; newObstacle->_length = abs( J1 - newObstacle->_point ); newObstacle->_id = _obstacles.size(); newObstacle->_class = obstacleJ->_class; _obstacles.push_back(newObstacle); obstacleJ->_nextObstacle = newObstacle; obstacleJ->_length = abs( J0 - newObstacle->_point ); if (j1LeftOfI > 0.0f) { leftObstacles[leftCounter++] = obstacleJ; rightObstacles[rightCounter++] = newObstacle; } else { rightObstacles[rightCounter++] = obstacleJ; leftObstacles[leftCounter++] = newObstacle; } } } node->_obstacle = obstacleI; node->_left = buildTreeRecursive(leftObstacles); node->_right = buildTreeRecursive(rightObstacles); return node; } }
QModelIndex QtTreeCursorNavigation::previousCellInThisRow(QModelIndex const &index) const { return leftOf(index); }
/* 1.27 IntegrateHoles is used to integrate all holes of a face into the main cycle by creating "bridges". This usually yields an invalid face, it is exclusively used to triangulate a face. */ void Face::IntegrateHoles() { vector<Seg> allsegs = v; // First, get a list of all segments (inclusively all holes' segments) for (unsigned int i = 0; i < holes.size(); i++) { allsegs.insert(allsegs.end(), holes[i].v.begin(), holes[i].v.end()); } for (unsigned int h = 0; h < holes.size(); h++) { Face hole = holes[h]; // Integrate one hole after another if (hole.isEmpty()) continue; unsigned int i = 0, j; bool found = false; Pt s, e; Seg se; // Try to find a suitable bridge-segment by creating all segments // which connect the hole to the face and testing them for intersection // with any other segment while (!found && i < v.size()) { s = v[i].e; j = 0; while (!found && j < hole.v.size()) { e = hole.v[j].s; se = Seg(s, e); // A segment connecting the face to the hole bool intersects = false; for (unsigned int k = 0; k < allsegs.size(); k++) { if (se.intersects(allsegs[k])) { // We have found an intersection, so this segment is // not our bridge intersects = true; break; } } Seg tmp = v[(i+1)%v.size()]; if (leftOf(tmp.s, tmp.e, e) && !intersects) { // No intersection was found, so this is the bridge we use found = true; break; } j = j + 1; } if (!found) { i = i + 1; } } assert(found); // We should always be able to find a bridge // Now insert the bridge and the hole into the face-cycle vector<Seg> newsegs; // First copy the cycle from start to the begin of the bridge newsegs.insert(newsegs.end(), v.begin(), v.begin()+i+1); newsegs.push_back(Seg(s, e)); // Then add the bridge segment unsigned int n = hole.v.size(); for (unsigned int k = 0; k < n; k++) { // Now add the hole-segments clockwise Seg ns = hole.v[(n + j - k - 1) % n]; ns.ChangeDir(); // and change the orientation of each segment newsegs.push_back(ns); } newsegs.push_back(Seg(e, s)); // Bridge back to the original face // and add the rest of the original cycle newsegs.insert(newsegs.end(), v.begin() + i + 1, v.end()); v = newsegs; // For the next holes we have to test intersection with the newly // created bridge, too. allsegs.push_back(Seg(s, e)); } // All holes were integrated, so clear the list. holes.clear(); }
/* 1.10 ConvexHull calculates the convex hull of this face and returns it as a new face. Corner-points on the convex hull are treated as members of the hull. */ Face Face::ConvexHull() { if (isEmpty()) return Face(); // erase the previously calculated hull convexhull.erase(convexhull.begin(), convexhull.end()); vector<Pt> lt = getPoints(); // Sort the points by the y-axis, begin with the lowest-(leftmost) point std::sort(lt.begin(), lt.end()); // Now calculate the polar coordinates (angle and distance of each point // with the lowest-(leftmost) point as origin. for (unsigned int a = 1; a < lt.size(); a++) { lt[a].calcPolar(lt[0]); } // Sort the other points by their angle (startpoint lt[0] remains in place) std::sort(lt.begin()+1, lt.end(), sortAngle); // since we want the corner-points on the hull, too, we need to list the // points with the lowest angle in counter-clockwise order. We sorted with // descending distance as second criterion, so we need to reverse these. // For that reason the points with the highest angle are already in // counter-clockwise order. std::vector<Pt>::iterator s = lt.begin() + 1, e = s; // Start with the // second point and find the last point with the same angle. while (s->angle == e->angle) e++; std::reverse(s, e); // Now reverse these to get the correct order. // This is the main part of the Graham scan. Initially, take the first two // points and put them on a stack vector<Pt> uh = vector<Pt> (); uh.push_back(lt[0]); uh.push_back(lt[1]); for (int a = 2; a < (int) lt.size();) { assert(uh.size() >= 2); // If the stack shrinks to 1, something went // ugly wrong. // Examine the two points on top of the stack ... Pt point1 = uh[uh.size() - 1]; Pt point2 = uh[uh.size() - 2]; Pt next = lt[a]; // ... and the next candidate for the hull if (leftOf(point2, point1, next)) { // If the candidate is left of (or on) the segment made up of the // two topmost points on the stack, we assume it is part of the hull uh.push_back(next); // and push it on the stack a++; } else { uh.pop_back(); // Otherwise it is right of the hull and the topmost // point is definitively not part of the hull, so // we kick it. } } // Now make a face from the points on the convex hull, which the vector ~uh~ // now contains in counter-clockwise order. for (unsigned int i = 0; i < uh.size(); i++) { Pt p1 = uh[i]; Pt p2 = uh[(i + 1) % uh.size()]; Seg s = Seg(p1, p2); convexhull.push_back(s); } return Face(convexhull); }