/*------------------------------------------------------------------------------*/
void GW_GeodesicPath::AddVertexToPath( GW_GeodesicVertex& Vert )
{
pPrevFace_ = pCurFace_;
GW_Float rBestDistance = GW_INFINITE;
pCurFace_ = NULL;
GW_GeodesicVertex* pSelectedVert = NULL;
for( GW_VertexIterator it = Vert.BeginVertexIterator(); it!=Vert.EndVertexIterator(); ++it )
{
GW_GeodesicVertex* pVert = (GW_GeodesicVertex*) *it;
if( pVert->GetDistance()<rBestDistance )
{
rBestDistance = pVert->GetDistance();
pSelectedVert = pVert;
GW_GeodesicVertex* pVert1 = (GW_GeodesicVertex*) it.GetLeftVertex();
GW_GeodesicVertex* pVert2 = (GW_GeodesicVertex*) it.GetRightVertex();
if( pVert1!=NULL && pVert2!=NULL )
{
if( pVert1->GetDistance()<pVert2->GetDistance() )
pCurFace_ = (GW_GeodesicFace*) it.GetLeftFace();
else
pCurFace_ = (GW_GeodesicFace*) it.GetRightFace();
}
else if( pVert1!=NULL )
{
pCurFace_ = (GW_GeodesicFace*) it.GetLeftFace();
}
else
{
GW_ASSERT( pVert2!=NULL );
pCurFace_ = (GW_GeodesicFace*) it.GetRightFace();
}
}
}
GW_ASSERT( pCurFace_!=NULL );
GW_ASSERT( pSelectedVert!=NULL );
GW_GeodesicPoint* pPoint = new GW_GeodesicPoint;
Path_.push_back( pPoint );
pPoint->SetVertex1( Vert );
pPoint->SetVertex2( *pSelectedVert );
pPoint->SetCoord(1);
pPoint->SetCurFace( *pCurFace_ );
}
/*------------------------------------------------------------------------------*/
void GW_GeodesicDisplayer::DisplayPath( GW_GeodesicPath& CurPath, GW_Vector3D& Color, GW_Float rLineWidth )
{
glLineWidth( (GLfloat) rLineWidth );
glPointSize( (GLfloat) rLineWidth );
T_GeodesicPointList& PointList = CurPath.GetPointList();
glDisable( GL_LIGHTING );
glColor( Color );
glBegin( GL_LINE_STRIP );
for( IT_GeodesicPointList it = PointList.begin(); it!=PointList.end(); ++it )
{
GW_GeodesicPoint* pPoint = *it;
GW_ASSERT( pPoint->GetVertex1()!=NULL );
GW_ASSERT( pPoint->GetVertex2()!=NULL );
GW_Vector3D Pos = pPoint->GetVertex1()->GetPosition()*pPoint->GetCoord() +
pPoint->GetVertex2()->GetPosition()*(1-pPoint->GetCoord());
glColor( Color );
glVertex( Pos );
GW_GeodesicFace* pFace = pPoint->GetCurFace();
GW_ASSERT( pFace!=NULL );
T_SubPointVector& SubPointVector = pPoint->GetSubPointVector();
GW_Vector3D& v0 = pPoint->GetVertex1()->GetPosition();
GW_Vector3D& v1 = pPoint->GetVertex2()->GetPosition();
GW_Vertex* pLastVert = pFace->GetVertex( *pPoint->GetVertex1(), *pPoint->GetVertex2() );
GW_ASSERT( pLastVert!=NULL );
GW_Vector3D& v2 = pLastVert->GetPosition();
for( IT_SubPointVector it=SubPointVector.begin(); it!=SubPointVector.end(); ++it )
{
GW_Vector3D& coord = *it;
Pos = v0*coord[0] + v1*coord[1] + v2*coord[2];
glVertex( Pos );
}
}
glEnd();
#if 0
glColor3f( 0,0,0 );
glPointSize(4);
glBegin( GL_POINTS );
for( IT_GeodesicPointList it = PointList.begin(); it!=PointList.end(); ++it )
{
GW_GeodesicPoint* pPoint = *it;
GW_ASSERT( pPoint->GetVertex1()!=NULL );
GW_ASSERT( pPoint->GetVertex2()!=NULL );
GW_Vector3D Pos = pPoint->GetVertex1()->GetPosition()*pPoint->GetCoord() +
pPoint->GetVertex2()->GetPosition()*(1-pPoint->GetCoord());
glVertex( Pos );
GW_GeodesicFace* pFace = pPoint->GetCurFace();
GW_ASSERT( pFace!=NULL );
T_SubPointVector& SubPointVector = pPoint->GetSubPointVector();
GW_Vector3D& v0 = pPoint->GetVertex1()->GetPosition();
GW_Vector3D& v1 = pPoint->GetVertex2()->GetPosition();
GW_Vertex* pLastVert = pFace->GetVertex( *pPoint->GetVertex1(), *pPoint->GetVertex2() );
GW_ASSERT( pLastVert!=NULL );
GW_Vector3D& v2 = pLastVert->GetPosition();
for( IT_SubPointVector it=SubPointVector.begin(); it!=SubPointVector.end(); ++it )
{
GW_Vector3D& coord = *it;
Pos = v0*coord[0] + v1*coord[1] + v2*coord[2];
glVertex( Pos );
}
}
glEnd();
glEnable( GL_LIGHTING );
glLineWidth( 1 );
#endif
}
/*------------------------------------------------------------------------------*/
GW_I32 GW_GeodesicPath::AddNewPoint()
{
// rStepSize_ = 1000;
GW_ASSERT( pCurFace_!=NULL );
GW_ASSERT( !Path_.empty() );
GW_GeodesicPoint* pPoint = Path_.back();
GW_ASSERT( pPoint!=NULL );
GW_GeodesicVertex* pVert1 = pPoint->GetVertex1();
GW_ASSERT( pVert1!=NULL );
GW_GeodesicVertex* pVert2 = pPoint->GetVertex2();
GW_ASSERT( pVert2!=NULL );
GW_GeodesicVertex* pVert3 = (GW_GeodesicVertex*) pCurFace_->GetVertex( *pVert1, *pVert2 );
GW_ASSERT( pVert3!=NULL );
/* compute coords of the last point */
T_SubPointVector& SubPointVector = pPoint->GetSubPointVector();
GW_Float x,y,z; // barycentric coords of the point
/* the sub-points should be empty */
GW_ASSERT( SubPointVector.empty() );
/* we begin on an edge */
x = pPoint->GetCoord();
y = 1-x;
z = 0;
GW_Float l1 = ~( pVert1->GetPosition() - pVert3->GetPosition() );
GW_Float l2 = ~( pVert2->GetPosition() - pVert3->GetPosition() );
pCurFace_->SetUpTriangularInterpolation();
GW_U32 nNum = 0;
while( nNum<1000 ) // never stop, this is just to avoid infinite loop
{
nNum++;
GW_Float dx, dy;
pCurFace_->ComputeGradient( *pVert1, *pVert2, *pVert3, x, y, dx, dy );
GW_Float l, a;
/* Try each kind of possible crossing.
The barycentric coords of the point is (x-l*dx/l1,y-l*dy/l2,z+l*(dx/l1+dy/l2)) */
if( GW_ABS(dx)>GW_EPSILON )
{
l = l1*x/dx; // position along the line
a = y-l*dy/l2; // coordonate with respect to v2
if( l>0 && l<=rStepSize_ && 0<=a && a<=1 )
{
/* the crossing occurs on [v2,v3] */
GW_GeodesicPoint* pNewPoint = new GW_GeodesicPoint;
Path_.push_back( pNewPoint );
pNewPoint->SetVertex1( *pVert2 );
pNewPoint->SetVertex2( *pVert3 );
pNewPoint->SetCoord( a );
pPrevFace_ = pCurFace_;
if( pCurFace_->GetFaceNeighbor( *pVert2, *pVert3 )==NULL )
{
pNewPoint->SetCurFace( *pCurFace_ );
/* check if we are *really* on a boundarie of the mesh */
GW_ASSERT( pCurFace_->GetEdgeNumber( *pVert2, *pVert3 )>=0 );
/* we should stay on the same face */
return 0;
}
pCurFace_ = (GW_GeodesicFace*) pCurFace_->GetFaceNeighbor( *pVert2, *pVert3 );
GW_ASSERT( pCurFace_!=NULL );
pNewPoint->SetCurFace( *pCurFace_ );
/* test for ending */
if( a<0.01 && pVert2->GetDistance()<GW_EPSILON )
return -1;
if( a>0.99 && pVert3->GetDistance()<GW_EPSILON )
return -1;
return 0;
}
}
if( GW_ABS(dy)>GW_EPSILON!=0 )
{
l = l2*y/dy; // position along the line
a = x-l*dx/l1; // coordonate with respect to v1
if( l>0 && l<=rStepSize_ && 0<=a && a<=1 )
{
/* the crossing occurs on [v1,v3] */
GW_GeodesicPoint* pNewPoint = new GW_GeodesicPoint;
Path_.push_back( pNewPoint );
pNewPoint->SetVertex1( *pVert1 );
pNewPoint->SetVertex2( *pVert3 );
pNewPoint->SetCoord( a );
pPrevFace_ = pCurFace_;
if( pCurFace_->GetFaceNeighbor( *pVert1, *pVert3 )==NULL )
{
pNewPoint->SetCurFace( *pCurFace_ );
/* check if we are *really* on a boundarie of the mesh */
GW_ASSERT( pCurFace_->GetEdgeNumber( *pVert1, *pVert3 )>=0 );
/* we should stay on the same face, the fact that pPrevFace_==pCurFace_
will force to go on an edge */
return 0;
}
pCurFace_ = (GW_GeodesicFace*) pCurFace_->GetFaceNeighbor( *pVert1, *pVert3 );
GW_ASSERT( pCurFace_!=NULL );
pNewPoint->SetCurFace( *pCurFace_ );
/* test for ending */
if( a<0.01 && pVert1->GetDistance()<GW_EPSILON )
return -1;
if( a>0.99 && pVert3->GetDistance()<GW_EPSILON )
return -1;
return 0;
}
}
if( GW_ABS(dx/l1+dy/l2)>GW_EPSILON )
{
l = -z/(dx/l1+dy/l2); // position along the line
a = x-l*dx/l1; // coordonate with respect to v1
if( l>0 && l<=rStepSize_ && 0<=a && a<=1 )
{
/* the crossing occurs on [v1,v2] */
GW_GeodesicPoint* pNewPoint = new GW_GeodesicPoint;
Path_.push_back( pNewPoint );
pNewPoint->SetVertex1( *pVert1 );
pNewPoint->SetVertex2( *pVert2 );
pNewPoint->SetCoord( a );
pPrevFace_ = pCurFace_;
if( pCurFace_->GetFaceNeighbor( *pVert1, *pVert2 )==NULL )
{
pNewPoint->SetCurFace( *pCurFace_ );
/* check if we are *really* on a boundarie of the mesh */
GW_ASSERT( pCurFace_->GetEdgeNumber( *pVert1, *pVert2 )>=0 );
/* we should stay on the same face */
return 0;
}
pCurFace_ = (GW_GeodesicFace*) pCurFace_->GetFaceNeighbor( *pVert1, *pVert2 );
GW_ASSERT( pCurFace_!=NULL );
pNewPoint->SetCurFace( *pCurFace_ );
/* test for ending */
if( a<0.01 && pVert1->GetDistance()<GW_EPSILON )
return -1;
if( a>0.99 && pVert2->GetDistance()<GW_EPSILON )
return -1;
return 0;
}
}
if( GW_ABS(dx)<GW_EPSILON && GW_ABS(dx)<GW_EPSILON )
{
/* special case : we must follow the edge. */
GW_GeodesicVertex* pSelectedVert = pVert1;
if( pVert2->GetDistance()<pSelectedVert->GetDistance() )
pSelectedVert = pVert2;
if( pVert3->GetDistance()<pSelectedVert->GetDistance() )
pSelectedVert = pVert3;
// just a check
this->AddVertexToPath( *pSelectedVert );
GW_ASSERT( pCurFace_!=NULL );
if( pSelectedVert->GetDistance()<GW_EPSILON )
return -1;
if( pCurFace_==pPrevFace_ && pPoint->GetCoord()>1-GW_EPSILON )
{
/* hum, problem, we are in a local minimum */
return -1;
}
return 0;
}
/* no intersection: we can advance */
GW_Float xprev = x, yprev = y;
x = x - rStepSize_*dx/l1;
y = y - rStepSize_*dy/l2;
if( x<0 || x>1 || y<0 || y>1 )
{
GW_ASSERT( z==0 );
GW_GeodesicFace* pNextFace = (GW_GeodesicFace*) pCurFace_->GetFaceNeighbor( *pVert3 );
if( pNextFace==pPrevFace_ || pCurFace_->GetFaceNeighbor( *pVert3 )==NULL )
{
/* special case : we must follow the edge. */
GW_GeodesicVertex* pSelectedVert = pVert1;
if( pVert2->GetDistance()<pSelectedVert->GetDistance() )
pSelectedVert = pVert2;
if( pVert3->GetDistance()<pSelectedVert->GetDistance() )
pSelectedVert = pVert3;
// just a check
this->AddVertexToPath( *pSelectedVert );
GW_ASSERT( pCurFace_!=NULL );
if( pSelectedVert->GetDistance()<GW_EPSILON )
return -1;
if( pCurFace_==pPrevFace_ && pPoint->GetCoord()>1-GW_EPSILON )
{
/* hum, problem, we are in a local minimum */
return -1;
}
// if( pSelectedVert==pVert3 )
// GW_ASSERT( x==0 || y==0 );
}
else
{
/* we should go on another face */
pPrevFace_ = pCurFace_;
pCurFace_ = (GW_GeodesicFace*) pCurFace_->GetFaceNeighbor( *pVert3 );
GW_ASSERT( pCurFace_!=NULL );
GW_GeodesicPoint* pNewPoint = new GW_GeodesicPoint;
Path_.push_back( pNewPoint );
pNewPoint->SetVertex1( *pVert1 );
pNewPoint->SetVertex2( *pVert2 );
pNewPoint->SetCurFace( *pCurFace_ );
pNewPoint->SetCoord( xprev );
}
return 0;
}
SubPointVector.push_back( GW_Vector3D(x,y,1-x-y) );
}
GW_ASSERT( GW_False );
GW_GeodesicVertex* pSelectedVert = pVert1;
if( pVert2->GetDistance()<pSelectedVert->GetDistance() )
pSelectedVert = pVert2;
if( pVert3->GetDistance()<pSelectedVert->GetDistance() )
pSelectedVert = pVert3;
// just a check
this->AddVertexToPath( *pSelectedVert );
GW_ASSERT( pCurFace_!=NULL );
if( pSelectedVert->GetDistance()<GW_EPSILON )
return -1;
if( pCurFace_==pPrevFace_ && pPoint->GetCoord()>1-GW_EPSILON )
{
/* hum, problem, we are in a local minimum */
return -1;
}
return 0;
}
/*------------------------------------------------------------------------------*/
void GW_VoronoiMesh::AddPathToMeshVertex( GW_GeodesicMesh& Mesh, GW_GeodesicPath& GeodesicPath, T_GeodesicVertexList& VertexPath )
{
GW_GeodesicVertex* pPrevVert = NULL;
T_GeodesicPointList& PointList = GeodesicPath.GetPointList();
for( IT_GeodesicPointList it=PointList.begin(); it!=PointList.end(); ++it )
{
GW_GeodesicPoint* pPoint = *it; GW_ASSERT( pPoint!=NULL );
GW_GeodesicVertex* pVert1 = pPoint->GetVertex1(); GW_ASSERT( pVert1!=NULL );
GW_GeodesicVertex* pVert2 = pPoint->GetVertex2(); GW_ASSERT( pVert2!=NULL );
GW_Float rCoord = pPoint->GetCoord();
GW_Bool bIsNewVertCreated;
GW_GeodesicVertex* pNewVert = (GW_GeodesicVertex*) Mesh.InsertVertexInEdge( *pPoint->GetVertex1(), *pPoint->GetVertex2(), pPoint->GetCoord(), bIsNewVertCreated );
if( bIsNewVertCreated )
{
pNewVert->SetDistance( pPoint->GetVertex1()->GetDistance()*rCoord +
pPoint->GetVertex2()->GetDistance()*(1-rCoord) );
pNewVert->SetState( pPoint->GetVertex1()->GetState() );
pNewVert->SetFront( pPoint->GetVertex1()->GetFront() );
}
/* add the vertex to the path */
VertexPath.push_back(pNewVert);
/* test for self intersection */
IT_GeodesicPointList itSelf = it;
itSelf++;
if( bIsNewVertCreated ) // fix only if new vertex is added
while( itSelf!=PointList.end() )
{
GW_GeodesicPoint* pPointSelf = *itSelf; GW_ASSERT( pPointSelf!=NULL );
GW_GeodesicVertex* pVertSelf1 = pPointSelf->GetVertex1(); GW_ASSERT( pVertSelf1!=NULL );
GW_GeodesicVertex* pVertSelf2 = pPointSelf->GetVertex2(); GW_ASSERT( pVertSelf1!=NULL );
GW_Float rCoordSelf = pPointSelf->GetCoord();
if( pVertSelf2==pVert1 && pVertSelf1==pVert2 )
{
/* we should swap the two vertex */
GW_GeodesicVertex* pTemp = pVertSelf1;
pVertSelf1 = pVertSelf2;
pVertSelf2 = pTemp;
rCoordSelf = 1-rCoordSelf;
}
if( pVertSelf2==pVert2 && pVertSelf1==pVert1 )
{
/* there is a self intersection */
if( rCoordSelf>rCoord )
{
pPointSelf->SetVertex1( *pVert1 );
pPointSelf->SetVertex2( *pNewVert );
pPointSelf->SetCoord( (rCoordSelf-rCoord)/(1-rCoord) );
}
else
{
pPointSelf->SetVertex1( *pNewVert );
pPointSelf->SetVertex2( *pVert2 );
if( rCoord>GW_EPSILON )
pPointSelf->SetCoord( rCoordSelf/rCoord );
else
pPointSelf->SetCoord(0);
}
}
itSelf++;
}
/* now test for intersection with previous path */
GW_U32 nID = GW_Vertex::ComputeUniqueId( *pVert1, *pVert2 );
/* to avoid to reprocess already processed lists */
std::list<T_GeodesicVertexList*> PathToProccess;
IT_VertexPathMultiMultiMap itIntersectedPath = BoundaryEdgeMap_.find(nID);
if( bIsNewVertCreated ) // fix only if new vertex is added
while( itIntersectedPath!=BoundaryEdgeMap_.end() && itIntersectedPath->first==nID )
{
PathToProccess.push_back( itIntersectedPath->second );
itIntersectedPath++;
}
for( std::list<T_GeodesicVertexList*>::iterator itIntersectedPath=PathToProccess.begin(); itIntersectedPath!=PathToProccess.end(); ++itIntersectedPath )
{
/* oups, intersection, we should fix it */
T_GeodesicVertexList* pIntersectedPath = *itIntersectedPath;
GW_ASSERT( pIntersectedPath!=NULL );
/* find where the crossing occurs */
IT_GeodesicVertexList itIntersec = pIntersectedPath->begin();
GW_GeodesicVertex* pIntersect1 = NULL;
GW_GeodesicVertex* pIntersect2 = NULL;
while( !(itIntersec==pIntersectedPath->end()) )
{
pIntersect2 = *itIntersec;
if( (pIntersect1==pVert1 && pIntersect2==pVert2) ||
(pIntersect1==pVert2 && pIntersect2==pVert1) )
break; // we have found the position in the path
pIntersect1 = pIntersect2;
itIntersec++;
}
if( pIntersect1!=NULL && pIntersect2!=NULL && itIntersec!=pIntersectedPath->end() )
{
/* insert new vertex in path */
pIntersectedPath->insert( itIntersec, pNewVert );
/* divide previous edge in two */
// BoundaryEdgeMap_.erase( nID ); // \todo find the correct iterator and remove it
nID = GW_Vertex::ComputeUniqueId( *pVert1, *pNewVert );
BoundaryEdgeMap_.insert( std::pair<GW_U32, T_GeodesicVertexList*>( nID, pIntersectedPath ) );
nID = GW_Vertex::ComputeUniqueId( *pVert2, *pNewVert );
BoundaryEdgeMap_.insert( std::pair<GW_U32, T_GeodesicVertexList*>( nID, pIntersectedPath ) );
}
else
GW_ASSERT( GW_False );
}
/* add the small newly created edge in the map */
if( pPrevVert==NULL )
pPrevVert = pNewVert;
else
{
nID = GW_Vertex::ComputeUniqueId( *pPrevVert, *pNewVert );
pPrevVert = pNewVert;
BoundaryEdgeMap_.insert( std::pair<GW_U32, T_GeodesicVertexList*>( nID, &VertexPath ) );
}
}
}
