//----------------------------------------------------------------------- bool ConvexBody::findAndEraseEdgePair(const Vector3& vec, Polygon::EdgeMap& intersectionEdges, Vector3& vNext ) const { for (Polygon::EdgeMap::iterator it = intersectionEdges.begin(); it != intersectionEdges.end(); ++it) { if (it->first.positionEquals(vec)) { vNext = it->second; // erase found edge intersectionEdges.erase( it ); return true; // found! } else if (it->second.positionEquals(vec)) { vNext = it->first; // erase found edge intersectionEdges.erase( it ); return true; // found! } } return false; // not found! }
//----------------------------------------------------------------------- Polygon::EdgeMap ConvexBody::getSingleEdges() const { Polygon::EdgeMap edgeMap; // put all edges of all polygons into a list every edge has to be // walked in each direction once for ( size_t i = 0; i < getPolygonCount(); ++i ) { const Polygon& p = getPolygon( i ); for ( size_t j = 0; j < p.getVertexCount(); ++j ) { const Vector3& a = p.getVertex( j ); const Vector3& b = p.getVertex( ( j + 1 ) % p.getVertexCount() ); edgeMap.insert( Polygon::Edge( a, b ) ); } } // search corresponding parts Polygon::EdgeMap::iterator it; Polygon::EdgeMap::iterator itStart; Polygon::EdgeMap::const_iterator itEnd; while( !edgeMap.empty() ) { it = edgeMap.begin(); ++it; // start one element after itStart itStart = edgeMap.begin(); // the element to be compared with the others itEnd = edgeMap.end(); // beyond the last element bool bFound = false; for ( ; it != itEnd; ++it ) { if (itStart->first.positionEquals(it->second) && itStart->second.positionEquals(it->first)) { // erase itStart and it edgeMap.erase( it ); edgeMap.erase( itStart ); bFound = true; break; // found } } if ( bFound == false ) { break; // not all edges could be matched // body is not closed } } return edgeMap; }
//----------------------------------------------------------------------- void ConvexBody::extend(const Vector3& pt) { // Erase all polygons facing towards the point. For all edges that // are not removed twice (once in AB and once BA direction) build a // convex polygon (triangle) with the point. Polygon::EdgeMap edgeMap; for ( size_t i = 0; i < getPolygonCount(); ++i ) { const Vector3& normal = getNormal( i ); // direction of the point in regard to the polygon // the polygon is planar so we can take an arbitrary vertex Vector3 ptDir = pt - getVertex( i, 0 ); ptDir.normalise(); // remove polygon if dot product is greater or equals null. if ( normal.dotProduct( ptDir ) >= 0 ) { // store edges (copy them because if the polygon is deleted // its vertices are also deleted) storeEdgesOfPolygon( i, &edgeMap ); // remove polygon deletePolygon( i ); // decrement iterator because of deleted polygon --i; } } // point is already a part of the hull (point lies inside) if ( edgeMap.empty() ) return; // remove the edges that are twice in the list (once from each side: AB,BA) Polygon::EdgeMap::iterator it; // iterate from first to the element before the last one for (Polygon::EdgeMap::iterator itStart = edgeMap.begin(); itStart != edgeMap.end(); ) { // compare with iterator + 1 to end // don't need to skip last entry in itStart since omitted in inner loop it = itStart; ++it; bool erased = false; // iterate from itStart+1 to the element before the last one for ( ; it != edgeMap.end(); ++it ) { if (itStart->first.positionEquals(it->second) && itStart->second.positionEquals(it->first)) { edgeMap.erase(it); // increment itStart before deletion (iterator invalidation) Polygon::EdgeMap::iterator delistart = itStart++; edgeMap.erase(delistart); erased = true; break; // found and erased } } // increment itStart if we didn't do it when erasing if (!erased) ++itStart; } // use the remaining edges to build triangles with the point // the vertices of the edges are in ccw order (edgePtA-edgePtB-point // to form a ccw polygon) while ( !edgeMap.empty() ) { Polygon::EdgeMap::iterator mapIt = edgeMap.begin(); // build polygon it.first, it.second, point Polygon *p = allocatePolygon(); p->insertVertex(mapIt->first); p->insertVertex(mapIt->second); p->insertVertex( pt ); // attach polygon to body insertPolygon( p ); // erase the vertices from the list // pointers are now held by the polygon edgeMap.erase( mapIt ); } }