//-----------------------------------------------------------------------
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::clip( const Plane& pl, bool keepNegative )
{
if ( getPolygonCount() == 0 )
return;
// current will be used as the reference body
ConvexBody current;
current.moveDataFromBody(*this);
OgreAssert( this->getPolygonCount() == 0, "Body not empty!" );
OgreAssert( current.getPolygonCount() != 0, "Body empty!" );
// holds all intersection edges for the different polygons
Polygon::EdgeMap intersectionEdges;
// clip all polygons by the intersection plane
// add only valid or intersected polygons to *this
for ( size_t iPoly = 0; iPoly < current.getPolygonCount(); ++iPoly )
{
// fetch vertex count and ignore polygons with less than three vertices
// the polygon is not valid and won't be added
const size_t vertexCount = current.getVertexCount( iPoly );
if ( vertexCount < 3 )
continue;
// current polygon
const Polygon& p = current.getPolygon( iPoly );
// the polygon to assemble
Polygon *pNew = allocatePolygon();
// the intersection polygon (indeed it's an edge or it's empty)
Polygon *pIntersect = allocatePolygon();
// check if polygons lie inside or outside (or on the plane)
// for each vertex check where it is situated in regard to the plane
// three possibilities appear:
Plane::Side clipSide = keepNegative ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;
// - side is clipSide: vertex will be clipped
// - side is !clipSide: vertex will be untouched
// - side is NOSIDE: vertex will be untouched
Plane::Side *side = OGRE_ALLOC_T(Plane::Side, vertexCount, MEMCATEGORY_SCENE_CONTROL);
for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
{
side[ iVertex ] = pl.getSide( p.getVertex( iVertex ) );
}
// now we check the side combinations for the current and the next vertex
// four different combinations exist:
// - both points inside (or on the plane): keep the second (add it to the body)
// - both points outside: discard both (don't add them to the body)
// - first vertex is inside, second is outside: add the intersection point
// - first vertex is outside, second is inside: add the intersection point, then the second
for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
{
// determine the next vertex
size_t iNextVertex = ( iVertex + 1 ) % vertexCount;
const Vector3& vCurrent = p.getVertex( iVertex );
const Vector3& vNext = p.getVertex( iNextVertex );
// case 1: both points inside (store next)
if ( side[ iVertex ] != clipSide && // NEGATIVE or NONE
side[ iNextVertex ] != clipSide ) // NEGATIVE or NONE
{
// keep the second
pNew->insertVertex( vNext );
}
// case 3: inside -> outside (store intersection)
else if ( side[ iVertex ] != clipSide &&
side[ iNextVertex ] == clipSide )
{
// Do an intersection with the plane. We use a ray with a start point and a direction.
// The ray is forced to hit the plane with any option available (eigher current or next
// is the starting point)
// intersect from the outside vertex towards the inside one
Vector3 vDirection = vCurrent - vNext;
vDirection.normalise();
Ray ray( vNext, vDirection );
std::pair< bool, Real > intersect = ray.intersects( pl );
// store intersection
if ( intersect.first )
{
// convert distance to vector
Vector3 vIntersect = ray.getPoint( intersect.second );
// store intersection
pNew->insertVertex( vIntersect );
pIntersect->insertVertex( vIntersect );
}
}
// case 4: outside -> inside (store intersection, store next)
else if ( side[ iVertex ] == clipSide &&
side[ iNextVertex ] != clipSide )
{
// Do an intersection with the plane. We use a ray with a start point and a direction.
// The ray is forced to hit the plane with any option available (eigher current or next
// is the starting point)
// intersect from the outside vertex towards the inside one
Vector3 vDirection = vNext - vCurrent;
vDirection.normalise();
Ray ray( vCurrent, vDirection );
std::pair< bool, Real > intersect = ray.intersects( pl );
// store intersection
if ( intersect.first )
{
// convert distance to vector
Vector3 vIntersect = ray.getPoint( intersect.second );
// store intersection
pNew->insertVertex( vIntersect );
pIntersect->insertVertex( vIntersect );
}
pNew->insertVertex( vNext );
}
// else:
// case 2: both outside (do nothing)
}
// insert the polygon only, if at least three vertices are present
if ( pNew->getVertexCount() >= 3 )
{
// in case there are double vertices, remove them
pNew->removeDuplicates();
// in case there are still at least three vertices, insert the polygon
if ( pNew->getVertexCount() >= 3 )
{
this->insertPolygon( pNew );
}
else
{
// delete pNew because it's empty or invalid
freePolygon(pNew);
pNew = 0;
}
}
else
{
// delete pNew because it's empty or invalid
freePolygon(pNew);
pNew = 0;
}
// insert intersection polygon only, if there are two vertices present
if ( pIntersect->getVertexCount() == 2 )
{
intersectionEdges.insert( Polygon::Edge( pIntersect->getVertex( 0 ),
pIntersect->getVertex( 1 ) ) );
}
// delete intersection polygon
// vertices were copied (if there were any)
freePolygon(pIntersect);
pIntersect = 0;
// delete side info
OGRE_FREE(side, MEMCATEGORY_SCENE_CONTROL);
side = 0;
}
// if the polygon was partially clipped, close it
// at least three edges are needed for a polygon
if ( intersectionEdges.size() >= 3 )
{
Polygon *pClosing = allocatePolygon();
// Analyze the intersection list and insert the intersection points in ccw order
// Each point is twice in the list because of the fact that we have a convex body
// with convex polygons. All we have to do is order the edges (an even-odd pair)
// in a ccw order. The plane normal shows us the direction.
Polygon::EdgeMap::iterator it = intersectionEdges.begin();
// check the cross product of the first two edges
Vector3 vFirst = it->first;
Vector3 vSecond = it->second;
// remove inserted edge
intersectionEdges.erase( it );
Vector3 vNext;
// find mating edge
if (findAndEraseEdgePair(vSecond, intersectionEdges, vNext))
{
// detect the orientation
// the polygon must have the same normal direction as the plane and then n
Vector3 vCross = ( vFirst - vSecond ).crossProduct( vNext - vSecond );
bool frontside = ( pl.normal ).directionEquals( vCross, Degree( 1 ) );
// first inserted vertex
Vector3 firstVertex;
// currently inserted vertex
Vector3 currentVertex;
// direction equals -> front side (walk ccw)
if ( frontside )
{
// start with next as first vertex, then second, then first and continue with first to walk ccw
pClosing->insertVertex( vNext );
pClosing->insertVertex( vSecond );
pClosing->insertVertex( vFirst );
firstVertex = vNext;
currentVertex = vFirst;
#ifdef _DEBUG_INTERSECTION_LIST
std::cout << "Plane: n=" << pl.normal << ", d=" << pl.d << std::endl;
std::cout << "First inserted vertex: " << *next << std::endl;
std::cout << "Second inserted vertex: " << *vSecond << std::endl;
std::cout << "Third inserted vertex: " << *vFirst << std::endl;
#endif
}
// direction does not equal -> back side (walk cw)
else
{
// start with first as first vertex, then second, then next and continue with next to walk ccw
pClosing->insertVertex( vFirst );
pClosing->insertVertex( vSecond );
pClosing->insertVertex( vNext );
firstVertex = vFirst;
currentVertex = vNext;
#ifdef _DEBUG_INTERSECTION_LIST
std::cout << "Plane: n=" << pl.normal << ", d=" << pl.d << std::endl;
std::cout << "First inserted vertex: " << *vFirst << std::endl;
std::cout << "Second inserted vertex: " << *vSecond << std::endl;
std::cout << "Third inserted vertex: " << *next << std::endl;
#endif
}
// search mating edges that have a point in common
// continue this operation as long as edges are present
while ( !intersectionEdges.empty() )
{
if (findAndEraseEdgePair(currentVertex, intersectionEdges, vNext))
{
// insert only if it's not the last (which equals the first) vertex
if ( !intersectionEdges.empty() )
{
currentVertex = vNext;
pClosing->insertVertex( vNext );
}
}
else
{
// degenerated...
break;
}
} // while intersectionEdges not empty
// insert polygon (may be degenerated!)
this->insertPolygon( pClosing );
}
// mating intersection edge NOT found!
else
{
freePolygon(pClosing);
}
} // if intersectionEdges contains more than three elements
}
//-----------------------------------------------------------------------
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 );
}
}