/*------------------------------------------------------------------------------*/
void GW_TriangularInterpolation_Linear::ComputeGradient( GW_GeodesicVertex& v0, GW_GeodesicVertex& v1, GW_GeodesicVertex& v2, GW_Float x, GW_Float y, GW_Float& dx, GW_Float& dy )
{
(void) x;
(void) y;
GW_Float d0 = v0.GetDistance();
GW_Float d1 = v1.GetDistance();
GW_Float d2 = v2.GetDistance();
/* compute gradient */
GW_Vector3D e0 = v0.GetPosition() - v2.GetPosition();
GW_Vector3D e1 = v1.GetPosition() - v2.GetPosition();
GW_Float l0 = e0.Norm();
GW_Float l1 = e1.Norm();
e0 /= l0;
e1 /= l1;
GW_Float dot = e0*e1;
/* The gradient in direction (e1,e2) is:
|<grad(d),e0>| |(d0-d2)/l0| |gu|
D = |<grad(d),e1>| = |(d1-d2)/l1| = |gv|
We are searching for grad(d) = dx e0 + dy e1 wich gives rise to the system :
| 1 dot| |dx|
|dot 1 | * |dy| = D where dot=<e0,e2>
ie it is:
1/det * | 1 -dot|*|gu|
|-dot 1 | |gv|
*/
GW_Float det = 1-dot*dot;
GW_ASSERT( det!=0 );
GW_Float gu = (d0-d2)/l0;
GW_Float gv = (d1-d2)/l1;
dx = 1/det * ( gu - dot*gv );
dy = 1/det * (-dot*gu + gv );
}
/*------------------------------------------------------------------------------*/
GW_Face* GW_VoronoiMesh::FindMaxFace( GW_GeodesicVertex& Vert )
{
GW_Float rBestDistance = -GW_INFINITE;
GW_Face* pCurFace_ = NULL;
for( GW_VertexIterator it = Vert.BeginVertexIterator(); it!=Vert.EndVertexIterator(); ++it )
{
GW_GeodesicVertex* pVert = (GW_GeodesicVertex*) *it;
if( pVert->GetDistance()>rBestDistance )
{
rBestDistance = pVert->GetDistance();
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_ = it.GetLeftFace();
else
pCurFace_ = it.GetRightFace();
}
else if( pVert1!=NULL )
{
pCurFace_ = it.GetLeftFace();
}
else
{
GW_ASSERT( pVert2!=NULL );
pCurFace_ = it.GetRightFace();
}
}
}
return pCurFace_;
}
/*------------------------------------------------------------------------------*/
void GW_VoronoiMesh::BuildGeodesicParametrization( GW_GeodesicMesh& Mesh )
{
Mesh.RegisterForceStopCallbackFunction( FastMarchingCallbackFunction_Parametrization );
Mesh.ResetParametrizationData();
/* for each base vertex, perform front propagation on the surrounding faces */
for( GW_U32 i=0; i<this->GetNbrVertex(); ++i )
{
CurrentTargetVertex_.clear();
pCurrentVoronoiVertex_ = (GW_VoronoiVertex*) this->GetVertex(i); // set up global data for callback function
GW_ASSERT( pCurrentVoronoiVertex_!=NULL );
GW_OutputComment("Performing local fast marching.");
this->PerformLocalFastMarching( Mesh, *pCurrentVoronoiVertex_ );
/* register geodesic boundaries length */
for( GW_VertexIterator it=pCurrentVoronoiVertex_->BeginVertexIterator(); it!=pCurrentVoronoiVertex_->EndVertexIterator(); ++it )
{
GW_VoronoiVertex* pNeighbor = (GW_VoronoiVertex*) *it;
GW_ASSERT( pNeighbor!=NULL );
GW_GeodesicVertex* pGeodesicVert = pNeighbor->GetBaseVertex();
GW_ASSERT( pGeodesicVert!=NULL );
GW_U32 nID = GW_Vertex::ComputeUniqueId( *pCurrentVoronoiVertex_, *pNeighbor );
if( GeodesicDistanceMap_.find(nID)==GeodesicDistanceMap_.end() )
{
GW_ASSERT( pGeodesicVert->GetState()==GW_GeodesicVertex::kDead );
/* this is the first time the distance is computed */
if( pGeodesicVert->GetState()==GW_GeodesicVertex::kDead )
GeodesicDistanceMap_[nID] = pGeodesicVert->GetDistance();
}
else
{
GW_ASSERT( pGeodesicVert->GetState()==GW_GeodesicVertex::kDead );
if( pGeodesicVert->GetState()==GW_GeodesicVertex::kDead )
{
/* distance alreay computed in the reverse direction : take the average */
GW_Float rPrevDist = GeodesicDistanceMap_[nID];
GW_Float rNewDist = pGeodesicVert->GetDistance();
GeodesicDistanceMap_[nID] = GW_MIN(rPrevDist, rNewDist); //(rPrevDist+rNewDist)*0.5;
}
}
}
}
/* now really compute the 3 parameters for each vertex */
GW_OutputComment("Computing geodesic parametrisation.");
this->ComputeVertexParameters( Mesh );
pCurrentVoronoiVertex_ = NULL;
Mesh.RegisterForceStopCallbackFunction( NULL );
}
/*------------------------------------------------------------------------------*/
GW_GeodesicVertex* GW_VoronoiMesh::FindMaxVertex( GW_GeodesicMesh& Mesh )
{
GW_GeodesicVertex* pSelectedVert = NULL;
GW_Float rMaxDist = -GW_INFINITE;
for( GW_U32 i=0; i<Mesh.GetNbrVertex(); ++i )
{
GW_GeodesicVertex* pVert = (GW_GeodesicVertex*) Mesh.GetVertex( i );
GW_ASSERT( pVert!=NULL );
if( pVert->GetFace()!=NULL && pVert->GetDistance()>rMaxDist )
{
rMaxDist = pVert->GetDistance();
pSelectedVert = pVert;
}
}
return pSelectedVert;
}
/*------------------------------------------------------------------------------*/
void GW_GeodesicDisplayer::SetUpDraw(GW_Mesh& Mesh)
{
GW_BasicDisplayer::SetUpDraw( Mesh );
GW_ASSERT( strcmp(Mesh.GetClassName().c_str(), "GW_GeodesicMesh")==0 );
aMinValue_[kGeodesicDistance] = GW_INFINITE;
aMaxValue_[kGeodesicDistance] = -GW_INFINITE;
for( GW_U32 i = 0; i<Mesh.GetNbrVertex(); ++i )
{
GW_GeodesicVertex* pVert = (GW_GeodesicVertex*) Mesh.GetVertex(i);
if( pVert->GetDistance()<GW_INFINITE*0.5 )
{
if( pVert->GetDistance()<aMinValue_[kGeodesicDistance] )
aMinValue_[kGeodesicDistance] = pVert->GetDistance();
if( pVert->GetDistance()>aMaxValue_[kGeodesicDistance] )
aMaxValue_[kGeodesicDistance] = pVert->GetDistance();
}
}
}
/*------------------------------------------------------------------------------*/
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_ );
}
/*------------------------------------------------------------------------------*/
GW_U32 GW_VoronoiMesh::AddFurthestPoint( T_GeodesicVertexList& VertList,GW_GeodesicMesh& Mesh, GW_Bool bUseRandomStartVertex )
{
if( VertList.empty() )
{
Mesh.ResetGeodesicMesh();
/* choose 1 point at random */
GW_GeodesicVertex* pStartVertex = (GW_GeodesicVertex*) Mesh.GetRandomVertex();
GW_ASSERT( pStartVertex!=NULL );
if( pStartVertex==NULL )
return 0;
VertList.push_back( pStartVertex );
if( !bUseRandomStartVertex )
{
/* find furthest point from this one */
GW_VoronoiMesh::PerformFastMarching( Mesh, VertList );
GW_GeodesicVertex* pSelectedVert = GW_VoronoiMesh::FindMaxVertex( Mesh );
GW_ASSERT( pSelectedVert!=NULL );
VertList.push_back( pSelectedVert );
VertList.pop_front(); // remove random choosen vertex
Mesh.ResetGeodesicMesh();
}
}
else
{
Mesh.RegisterVertexInsersionCallbackFunction( GW_VoronoiMesh::FastMarchingCallbackFunction_VertexInsersion );
/* perform marching */
GW_VoronoiMesh::ResetOnlyVertexState( Mesh );
GW_GeodesicVertex* pStartVert = VertList.back();
GW_ASSERT( pStartVert!=NULL );
Mesh.PerformFastMarching( pStartVert );
/* find intersection points */
GW_GeodesicVertex* pSelectedVert = GW_VoronoiMesh::FindMaxVertex( Mesh );
GW_ASSERT( pSelectedVert!=NULL );
/* find the triangle where the maximum occurs */
#if 0
GW_Face* pMaxFace = GW_VoronoiMesh::FindMaxFace( *pSelectedVert );
GW_ASSERT( pMaxFace!=NULL );
/* this is a critical point */
GW_Float x=1.0f/3, y=1.0f/3, z=1.0f/3;
GW_GeodesicVertex* pNewVert = (GW_GeodesicVertex*) Mesh.InsertVertexInFace( *pMaxFace, x,y,z );
pNewVert->SetFront( pSelectedVert->GetFront() );
pNewVert->SetDistance( pSelectedVert->GetDistance() );
#else
GW_GeodesicVertex* pNewVert = pSelectedVert;
#endif
/* compute the real point */
VertList.push_back( pNewVert );
Mesh.RegisterVertexInsersionCallbackFunction( NULL );
}
return 1;
}
GW_Bool StopMarchingCallback( GW_GeodesicVertex& Vert )
{
// check if the end point has been reached
GW_U32 i = Vert.GetID();
// display_message("ind %d",i );
// display_message("dist %f",Vert.GetDistance() );
if( Vert.GetDistance()>dmax )
return true;
for( int k=0; k<nend; ++k )
if( end_points[k]==i )
return true;
return false;
}
/*------------------------------------------------------------------------------*/
GW_Bool GW_VoronoiMesh::FastMarchingCallbackFunction_Parametrization( GW_GeodesicVertex& CurVert )
{
GW_ASSERT( pCurrentVoronoiVertex_!=NULL );
if( !CurVert.GetIsStoppingVertex() || !CurVert.GetBoundaryReached() )
{
/* add the parameter value */
CurVert.AddParameterVertex( *pCurrentVoronoiVertex_, CurVert.GetDistance() );
if( CurVert.GetIsStoppingVertex() )
CurVert.SetBoundaryReached( GW_True );
}
/* always continue the computations */
return GW_False;
}
/*------------------------------------------------------------------------------*/
void GW_TriangularInterpolation_Cubic::ComputeLocalGradient( GW_GeodesicVertex& Vert )
{
/* compute the total angle */
GW_Vector3D PrevEdge;
GW_Float rTotalAngle = 0;
for( GW_VertexIterator it=Vert.BeginVertexIterator(); it!=Vert.EndVertexIterator(); ++it )
{
GW_Vertex* pVert = *it;
GW_ASSERT( pVert!=NULL );
if( it==Vert.BeginVertexIterator() )
{
PrevEdge = pVert->GetPosition() - Vert.GetPosition();
PrevEdge.Normalize();
}
else
{
GW_Vector3D NextEdge = pVert->GetPosition() - Vert.GetPosition();
NextEdge.Normalize();
rTotalAngle += acos( NextEdge*PrevEdge );
PrevEdge = NextEdge;
}
}
/* matrix and RHS for least square minimusation */
GW_Float M[2][2] = {{0,0},{0,0}};
GW_Float b[2] = {0,0};
GW_Float rCurAngle = 0;
PrevEdge.SetZero();
for( GW_VertexIterator it=Vert.BeginVertexIterator(); it!=Vert.EndVertexIterator(); ++it )
{
GW_GeodesicVertex* pVert = (GW_GeodesicVertex*) *it;
GW_ASSERT( pVert!=NULL );
GW_Vector3D Edge = pVert->GetPosition() - Vert.GetPosition();
GW_Float a = Edge.Norm();
Edge /= a;
if( it!=Vert.BeginVertexIterator() )
{
/* update the angle */
rCurAngle += acos( Edge*PrevEdge );
}
/* directional gradient estimation */
GW_Float delta = (pVert->GetDistance() - Vert.GetDistance())/a;
/* coordonate of the edge on (u,v) [flatened coords] */
GW_Float eu = a*cos( rCurAngle/rTotalAngle );
GW_Float ev = a*sin( rCurAngle/rTotalAngle );
/* update the matrix */
M[0][0] += eu*eu;
M[0][1] += eu*ev;
M[1][0] += eu*ev;
M[1][1] += ev*ev;
b[0] += delta*eu;
b[1] += delta*ev;
PrevEdge = Edge;
}
/* invert the system */
GW_Float det = M[0][0]*M[1][1] - M[0][1]*M[1][0];
GW_ASSERT( det!=0 );
GW_Float gu = 1/det * ( M[1][1]*b[0] - M[0][1]*b[1] );
GW_Float gv = 1/det * (-M[1][0]*b[0] + M[0][0]*b[1] );
/* set the gradient in local coords for each surrounding face */
rCurAngle = 0;
for( GW_FaceIterator it = Vert.BeginFaceIterator(); it!=Vert.EndFaceIterator(); ++it )
{
GW_GeodesicFace* pFace = (GW_GeodesicFace*) *it;
GW_ASSERT( pFace!=NULL );
GW_Vertex* pVert1 = it.GetLeftVertex(); GW_ASSERT( pVert1!=NULL );
GW_Vertex* pVert2 = it.GetRightVertex(); GW_ASSERT( pVert1!=NULL );
GW_Vector3D e1 = pVert1->GetPosition() - Vert.GetPosition();
GW_Vector3D e2 = pVert2->GetPosition() - Vert.GetPosition();
GW_Float a1 = e1.Norm();
GW_Float a2 = e2.Norm();
e1 /= a1;
e2 /= a2;
GW_Float rInnerAngle = acos( e1*e2 );
/* flattened position of the two vertex */
GW_Float p1[2], p2[2];
p1[0] = cos( rCurAngle );
p1[1] = sin( rCurAngle );
p2[0] = cos( rCurAngle+rInnerAngle );
p2[1] = sin( rCurAngle+rInnerAngle );
/* we have grad = gu*u + gv*v
we are searching for grad = g1*p1 + g2*p2, so:
gu = g1*<p1,u> + g2*<p2,u>
gv = g1*<p1,v> + g2*<p2,v>
i.e.
|p1[0] p2[0]| |g1| |gu|
|p1[1] p2[1]|*|g2| = |gv|
*/
det = p1[0]*p2[1]-p1[1]*p2[0];
GW_ASSERT( det!=0 );
GW_Float g1 = 1/det * ( p2[1]*gu - p2[0]*gv );
GW_Float g2 = 1/det * (-p1[1]*gu + p1[0]*gv );
/* now compute the gradient in world coords */
GW_Vector3D LocGrad = e1*g1 + e2*g2;
GW_TriangularInterpolation_ABC* pInterp = pFace->GetTriangularInterpolation();
if( pInterp==NULL )
{
pInterp = new GW_TriangularInterpolation_Cubic;
pFace->SetTriangularInterpolation( *pInterp );
}
GW_ASSERT( pInterp->GetType()==kCubicTriangulationInterpolation );
((GW_TriangularInterpolation_Cubic*) pInterp)->SetLocalGradient( LocGrad, *pFace, Vert );
rCurAngle += rInnerAngle;
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray*prhs[] )
{
nbr_iter = 0;
/* retrive arguments */
if( nrhs<6 )
mexErrMsgTxt("6 or 7 input arguments are required.");
if( nlhs<1 )
mexErrMsgTxt("1 or 2 output arguments are required.");
// arg1 : vertex
vertex = mxGetPr(prhs[0]);
nverts = mxGetN(prhs[0]);
if( mxGetM(prhs[0])!=3 )
mexErrMsgTxt("vertex must be of size 3 x nverts.");
// arg2 : faces
faces = mxGetPr(prhs[1]);
nfaces = mxGetN(prhs[1]);
if( mxGetM(prhs[1])!=3 )
mexErrMsgTxt("face must be of size 3 x nfaces.");
// arg3 : W
Ww = mxGetPr(prhs[2]);
int m = mxGetM(prhs[2]);
if( m!=nverts )
mexErrMsgTxt("W must be of same size as vertex.");
// arg4 : start_points
start_points = mxGetPr(prhs[3]);
nstart = mxGetM(prhs[3]);
// arg5 : end_points
end_points = mxGetPr(prhs[4]);
nend = mxGetM(prhs[4]);
// arg6 : niter_max
niter_max = (int) *mxGetPr(prhs[5]);
// arg7 : H
if( nrhs>=7 )
{
H = mxGetPr(prhs[6]);
int m =mxGetM(prhs[6]);
if( m>0 && m!=nverts )
mexErrMsgTxt("H must be of size nverts.");
if( m==0 )
H = NULL;
}
else
{
H = NULL;
}
// arg8 : L
if( nrhs>=8 )
{
L = mxGetPr(prhs[7]);
int m =mxGetM(prhs[7]);
if( m>0 && mxGetM(prhs[7])!=nverts )
mexErrMsgTxt("L must be of size nverts.");
if( m==0 )
L = NULL;
}
else
L = NULL;
// argument 9: value list
if( nrhs>=9 )
{
values = mxGetPr(prhs[8]);
if( mxGetM(prhs[8])==0 && mxGetN(prhs[8])==0 )
values=NULL;
if( values!=NULL && (mxGetM(prhs[8])!=nstart || mxGetN(prhs[8])!=1) )
mexErrMsgTxt("values must be of size nb_start_points x 1.");
}
else
values = NULL;
// argument 10: dmax
if( nrhs>=9 )
dmax = *mxGetPr(prhs[9]);
else
dmax = 1e9;
// first ouput : distance
plhs[0] = mxCreateDoubleMatrix(nverts, 1, mxREAL);
D = mxGetPr(plhs[0]);
// second output : state
plhs[1] = mxCreateDoubleMatrix(nverts, 1, mxREAL);
S = mxGetPr(plhs[1]);
// second output : segmentation
plhs[2] = mxCreateDoubleMatrix(nverts, 1, mxREAL);
Q = mxGetPr(plhs[2]);
// create the mesh
GW_GeodesicMesh Mesh;
Mesh.SetNbrVertex(nverts);
for( int i=0; i<nverts; ++i )
{
GW_GeodesicVertex& vert = (GW_GeodesicVertex&) Mesh.CreateNewVertex();
vert.SetPosition( GW_Vector3D(vertex_(0,i),vertex_(1,i),vertex_(2,i)) );
Mesh.SetVertex(i, &vert);
}
Mesh.SetNbrFace(nfaces);
for( int i=0; i<nfaces; ++i )
{
GW_GeodesicFace& face = (GW_GeodesicFace&) Mesh.CreateNewFace();
GW_Vertex* v1 = Mesh.GetVertex((int) faces_(0,i)); GW_ASSERT( v1!=NULL );
GW_Vertex* v2 = Mesh.GetVertex((int) faces_(1,i)); GW_ASSERT( v2!=NULL );
GW_Vertex* v3 = Mesh.GetVertex((int) faces_(2,i)); GW_ASSERT( v3!=NULL );
face.SetVertex( *v1,*v2,*v3 );
Mesh.SetFace(i, &face);
}
Mesh.BuildConnectivity();
// set up fast marching
Mesh.ResetGeodesicMesh();
for( int i=0; i<nstart; ++i )
{
GW_GeodesicVertex* v = (GW_GeodesicVertex*) Mesh.GetVertex((GW_U32) start_points[i]);
GW_ASSERT( v!=NULL );
Mesh.AddStartVertex( *v );
}
Mesh.SetUpFastMarching();
Mesh.RegisterWeightCallbackFunction( WeightCallback );
Mesh.RegisterForceStopCallbackFunction( StopMarchingCallback );
Mesh.RegisterVertexInsersionCallbackFunction( InsersionCallback );
if( H!=NULL )
Mesh.RegisterHeuristicToGoalCallbackFunction( HeuristicCallback );
// initialize the distance of the starting points
if( values!=NULL )
for( int i=0; i<nstart; ++i )
{
GW_GeodesicVertex* v = (GW_GeodesicVertex*) Mesh.GetVertex((GW_U32) start_points[i]);
GW_ASSERT( v!=NULL );
v->SetDistance( values[i] );
}
// perform fast marching
// display_message("itermax=%d", niter_max);
Mesh.PerformFastMarching();
// output result
for( int i=0; i<nverts; ++i )
{
GW_GeodesicVertex* v = (GW_GeodesicVertex*) Mesh.GetVertex((GW_U32) i);
GW_ASSERT( v!=NULL );
D[i] = v->GetDistance();
S[i] = v->GetState();
GW_GeodesicVertex* v1 = v->GetFront();
if( v1==NULL )
Q[i] = -1;
else
Q[i] = v1->GetID();
}
return;
}
/*------------------------------------------------------------------------------*/
GW_Float GW_TriangularInterpolation_Linear::ComputeValue( GW_GeodesicVertex& v0, GW_GeodesicVertex& v1, GW_GeodesicVertex& v2, GW_Float x, GW_Float y )
{
return v0.GetDistance()*x + v1.GetDistance()*y + v2.GetDistance()*(1-x-y);
}
/*------------------------------------------------------------------------------*/
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;
}
/*------------------------------------------------------------------------------*/
GW_Bool GW_VoronoiMesh::FastMarchingCallbackFunction_VertexInsersion( GW_GeodesicVertex& CurVert, GW_Float rNewDist )
{
return CurVert.GetDistance() > rNewDist;
}
/*------------------------------------------------------------------------------*/
void GW_VoronoiMesh::FlattenBasePoints( GW_GeodesicMesh& Mesh, T_Vector2DMap& FlatteningMap )
{
char str[100];
/* create a voronoi vertex for each basis vertex */
this->CreateVoronoiVertex();
GW_U32 n = this->GetNbrVertex();
GW_MatrixNxP dist_matrix(n,n,-1);
/* compute the distance matrix */
for( GW_U32 i=0; i<n; ++i )
{
GW_VoronoiVertex* pVoronoiVerti = (GW_VoronoiVertex*) this->GetVertex(i); GW_ASSERT( pVoronoiVerti!=NULL );
GW_GeodesicVertex* pGeodesicVerti = (GW_GeodesicVertex*) pVoronoiVerti->GetBaseVertex(); GW_ASSERT( pGeodesicVerti!=NULL );
sprintf( str, "Computing distance information for vertex %d.", i );
GW_OutputComment( str );
/* compute the distance map */
Mesh.ResetGeodesicMesh();
Mesh.PerformFastMarching( pGeodesicVerti );
for( GW_U32 j=0; j<n; ++j )
{
GW_VoronoiVertex* pVoronoiVertj = (GW_VoronoiVertex*) this->GetVertex(j); GW_ASSERT( pVoronoiVerti!=NULL );
GW_GeodesicVertex* pGeodesicVertj = (GW_GeodesicVertex*) pVoronoiVertj->GetBaseVertex(); GW_ASSERT( pGeodesicVerti!=NULL );
GW_Float rDist = pGeodesicVertj->GetDistance();
if( dist_matrix[i][j]<0 )
dist_matrix[i][j] = rDist*rDist;
else
dist_matrix[i][j] = (dist_matrix[i][j]+rDist*rDist)*0.5;
if( dist_matrix[j][i]<0 )
dist_matrix[j][i] = rDist*rDist;
else
dist_matrix[j][i] = (dist_matrix[j][i]+rDist*rDist)*0.5;
}
}
/* center the matrix */
sprintf( str, "Performing the multidimensional scaling.", i );
GW_OutputComment( str );
GW_MatrixNxP J(n,n,-1.0/n);
for( GW_U32 i=0; i<n; ++i )
J[i][i] += 1;
/* center the matrix */
dist_matrix = J*dist_matrix*J*(-0.5);
/* perform eigen-decomposition */
GW_MatrixNxP v(n,n);
GW_VectorND RealEig(n);
dist_matrix.Eigenvalue( v, NULL, &RealEig, NULL );
GW_Float rScaleX = 0;
GW_Float rScaleY = 0;
GW_I32 nEigenvectorX = -1;
GW_I32 nEigenvectorY = -1;
/* find the two maximum eigenvalues */
for( GW_U32 i=0; i<n; ++i )
{
if( RealEig[i]>=rScaleX )
{
rScaleY = rScaleX;
nEigenvectorY = nEigenvectorX;
rScaleX = RealEig[i];
nEigenvectorX = i;
}
else if( RealEig[i]>=rScaleY )
{
rScaleY = RealEig[i];
nEigenvectorY = i;
}
}
rScaleX = sqrt(rScaleX);
rScaleY = sqrt(rScaleY);
GW_ASSERT( nEigenvectorX>=0 );
GW_ASSERT( nEigenvectorY>=0 );
/* compute the 2D position */
FlatteningMap.clear();
if( nEigenvectorX>=0 && nEigenvectorY>=0 )
for( GW_U32 i=0; i<n; ++i )
{
GW_VoronoiVertex* pVoronoiVerti = (GW_VoronoiVertex*) this->GetVertex(i); GW_ASSERT( pVoronoiVerti!=NULL );
GW_GeodesicVertex* pGeodesicVerti = (GW_GeodesicVertex*) pVoronoiVerti->GetBaseVertex(); GW_ASSERT( pGeodesicVerti!=NULL );
GW_Vector2D pos;
pos[0] = rScaleX*v[i][nEigenvectorX];
pos[1] = rScaleY*v[i][nEigenvectorY];
FlatteningMap[ pGeodesicVerti->GetID() ] = pos;
}
}
