const R3Point R3Box::
ClosestPoint(const R3Point& point) const
{
// Return closest point in box
R3Point closest(point);
if (closest.X() < XMin()) closest[RN_X] = XMin();
else if (closest.X() > XMax()) closest[RN_X] = XMax();
if (closest.Y() < YMin()) closest[RN_Y] = YMin();
else if (closest.Y() > YMax()) closest[RN_Y] = YMax();
if (closest.Z() < ZMin()) closest[RN_Z] = ZMin();
else if (closest.Z() > ZMax()) closest[RN_Z] = ZMax();
return closest;
}
template <class cCRNode> void cTplCoxRoyAlgo<cCRNode>::SetStdCostRegul(double aCoeff,double aCste,int aVmin)
{
for (int anX=X0(); anX<X1() ; anX++)
for (int anY=Y0(); anY<Y1() ; anY++)
for (int aZ = ZMin(anX,anY); aZ< ZMax(anX,anY) ; aZ++)
{
cRoyPt aP1 (anX,anY,aZ);
cCRNode & aS1 = NodeOfP(aP1);
int aC1 = aS1.ResidualFlow(mDirZPlus);
for (int anEdg=0; anEdg<NbEdges() ; anEdg++)
{
if (aS1.EdgeIsValide(anEdg) && (!tabCRIsVertical[anEdg]))
{
cRoyPt aP2(aP1,anEdg);
cCRNode & aS2 = NodeOfP(aP2);
int aC2 = aS2.ResidualFlow(mDirZPlus);
double aCost = aCste + aCoeff*(aC1+aC2)/2.0;
if (Cnx8())
aCost *= tabCRIsArcV8[anEdg] ? 0.2928 : 0.4142 ;
int iCost = int(aCost+0.5);
if (iCost<aVmin)
iCost = aVmin;
aS1.SetResidualFlow(anEdg,iCost);
}
}
}
}
/** relabel at the same time **/
template <class cCRNode> void cTplCoxRoyAlgo<cCRNode>::relabel(void)
{
mNbRelabel++; /* stats */
// printf("-- relabel --\n");
mCRQ.QueueReset();
/** first, flush the heap **/
mCRH.LinkQReset();
/*BackToSource=0;*/
/** all node are unlabelled **/
for (int anX =mX0Loc; anX<mX1Loc ; ++anX)
{
for (int anY =mY0Loc; anY<mY1Loc ; ++anY)
{
cCRNode * aCol = ColumnOfP(anX,anY);
int aZ0 = ZMin(anX,anY);
int aZ1 = ZMax(anX,anY);
for (int aZ = aZ0; aZ<aZ1 ; ++aZ)
{
aCol[aZ].SetCurrentEdge(0);
/** start by queueing all nodes adjacent to sink **/
if ( aCol[aZ].SinkConnected())
{
aCol[aZ].SetHeight(mSinkHeight+1);
mCRQ.QueueAdd(cRoyPt(anX,anY,aZ));
}
else
aCol[aZ].SetHeight(mNoLabel);
}
}
}
cRoyPt aPU(-1111,1111,6669); // BIDON
while(!mCRQ.empty() )
{
mCRQ.QueueRemove(aPU);
cCRNode & aNodU = NodeOfP(aPU);
/** add this node to list of node to process **/
if( aNodU.Excess()>0 ) mCRH.LinkQInsert(aPU,aNodU.Height());
/** check each edge for residual flow **/
for(int ed=0;ed<eNbEdges; ++ed)
if ( aNodU.EdgeIsValide(ed))
{
cRoyPt aPV (aPU,ed);
cCRNode & aNodV = NodeOfP(aPV);
if (UnLabelled(aNodV) && (aNodV.UnSatured(InverseEdgeTable[ed])))
{
aNodV.SetHeight(aNodU.Height()+1);
mCRQ.QueueAdd(aPV);
}
}
}
}
template <class cCRNode> int cTplCoxRoyAlgo<cCRNode>::init_sz()
{
int res =0;
mZMinGlob = ZMin(mX0Glob,mY0Glob);
mZMaxGlob = ZMax(mX0Glob,mY0Glob);
for(int anX=mX0Glob ; anX<mX1Glob ; anX++)
{
for(int anY=mY0Glob ; anY<mY1Glob ; anY++)
{
int aZ0 = ZMin(anX,anY);
int aZ1 = ZMax(anX,anY);
assert(aZ0<aZ1);
res += aZ1-aZ0;
if (aZ0 < mZMinGlob) mZMinGlob = aZ0;
if (aZ1 > mZMaxGlob) mZMaxGlob = aZ1;
}
}
return res;
}
/** allocate Sol (if non null) to (xs x ys) solution array **/
template <class cCRNode> void cTplCoxRoyAlgo<cCRNode>::findcut(short **Sol)
{
// printf("-- find cut --\n");
for(int anY=mY0Glob ; anY<mY1Glob ; anY++)
{
for(int anX=mX0Glob ; anX<mX1Glob ; anX++)
{
Sol[anY][anX]= ZMinGlob() -1;
int aZ0 = ZMin(anX,anY);
int aZ1 = ZMax(anX,anY);
Sol[anY][anX]=aZ0; //Bug qui fait que parfois le Z n'est pas trouve
for (int aZ = aZ0; aZ<aZ1 ; aZ++)
{
cRoyPt aPU(anX,anY,aZ);
cCRNode & aNodU = NodeOfP(aPU);
if(UnLabelled(aNodU))
{
for(int ed=0;ed<eNbEdges;ed++)
{
if ( aNodU.EdgeIsValide(ed))
{
cRoyPt aPV(aPU,ed);
cCRNode & aNodV = NodeOfP(aPV);
if(aNodV.Height()!=mNoLabel&&aNodV.Height()!=mNoLabelBorder)
{
aNodU.SetHeight(mNoLabelBorder);
Sol[anY][anX]=aZ;
}
}
}
}
}
}
}
}
template <class cCRNode> void cTplCoxRoyAlgo<cCRNode>::SetFlagEdges
(int aX0,int aY0,int aX1,int aY1,bool UpdateSinkSource)
{
for (int anX =aX0; anX<aX1 ; anX++)
{
for (int anY =aY0; anY<aY1 ; anY++)
{
cCRNode * aCol = ColumnOfP(anX,anY);
int aZ0 = ZMin(anX,anY);
int aZ1 = ZMax(anX,anY);
for (int aZ = aZ0; aZ<aZ1 ; aZ++)
aCol[aZ].SetAllEdgeValide();
aCol[aZ0].SetEdgeNotValide(mDirZMoins);
aCol[aZ1-1].SetEdgeNotValide(mDirZPlus);
if (UpdateSinkSource)
{
aCol[aZ0].SetSourceConnected();
aCol[aZ1-1].SetSinkConnected();
}
for (int ed=0 ; ed<eNbEdges ; ed++)
{
if (zdelta[ed]==0)
{
int vX = anX+ xdelta[ed];
int vY = anY+ ydelta[ed];
bool isInside = (vX>=aX0)&&(vX<aX1)&&(vY>=aY0)&&(vY<aY1);
int zMinUnval = aZ0;
int zMaxUnval = aZ0;
// Modif MPD-GM du 26/06/09 suite a Plantage lorsque les colonnes ne sont pas connectees
/*
if (isInside)
{
zMinUnval = ZMin(vX,vY);
zMaxUnval = ZMax(vX,vY);
}
*/
if (isInside)
{
zMinUnval = ElMin(aZ1,ZMin(vX,vY));
zMaxUnval = ElMax(aZ0,ZMax(vX,vY));
}
for (int aZ = aZ0; aZ<zMinUnval ; aZ++)
aCol[aZ].SetEdgeNotValide(ed);
{
for (int aZ = zMaxUnval; aZ<aZ1 ; aZ++)
aCol[aZ].SetEdgeNotValide(ed);
}
if (UpdateSinkSource && isInside)
{
for (int aZ = aZ0; aZ<zMinUnval ; aZ++)
aCol[aZ].SetSourceConnected();
{
for (int aZ = zMaxUnval; aZ<aZ1 ; aZ++)
aCol[aZ].SetSinkConnected();
}
}
}
}
}
}
}
inline int ZLength(int anX,int anY)const {return ZMax(anX,anY)-ZMin(anX,anY);}
void cTplCoxRoyAlgo<cCRNode>::EndMaxFlowStd(int aX0,int aY0,int aX1,int aY1)
{
mX0Loc = aX0;
mY0Loc = aY0;
mX1Loc = aX1;
mY1Loc = aY1;
SetFlagEdges(mX0Loc,mY0Loc,mX1Loc,mY1Loc,false);
// Init();
/** flood the first nodes **/
for(int anY=mY0Loc ; anY<mY1Loc ; anY++)
{
for(int anX=mX0Loc ; anX<mX1Loc ; anX++)
{
cCRNode * aCol = ColumnOfP(anX,anY);
int aZ0 = ZMin(anX,anY);
int aZ1 = ZMax(anX,anY);
for (int aZ = aZ0; (aZ<aZ1) && (aCol[aZ].SourceConnected()) ; aZ++)
{
aCol[aZ].SetExcess(largef);
mCRH.LinkQInsert(cRoyPt(anX,anY,aZ),aCol[aZ].Height());
}
}
}
ResizeDetected=false;
// End Init()
int Level=mCRH.MaxKey();
int Count =0;
// printf("Starting at Key=%d\n",Level);
/** generic preflow-push **/
for(;;)
{
/** This is the schedule for relabelling **/
/** it can be changed for more/less relabel steps **/
if( ((Count+1)%(2*sz/2)==0 && mSinkFlow!=0) || ResizeDetected )
{
relabel();
Level=mCRH.MaxKey();
ResizeDetected=false;
}
/** update level to a non empty key **/
mCRH.Set2NonEmptyKey(Level);
/* if reached bottom, reset to top */
if( Level<0 ) Level=mCRH.MaxKey(); /* highest level */
/* if Level still <0 -> list MUST be empty! */
if( Level<0 ) break; /* no more nodes! */
/** get overflowing node with largest level **/
cRoyPt aPU = mCRH.LinkQRemove(Level);
// if( Count%100000==0 )
// {
// mCRH.ShowLink();
// printf("%6dk... SinkFlow=%8d \n",Count/1000,mSinkFlow);
// }
Discharge(aPU);
Count++;
}
// printf("SinkFlow=%d\n",mSinkFlow);
}
template <class cCRNode> cTplCoxRoyAlgo<cCRNode>::cTplCoxRoyAlgo
(
int xsz,int ysz,
signed short ** aZMin, signed short ** aZMax
) :
cInterfaceCoxRoyAlgo(xsz,ysz,aZMin,aZMax,eNbEdges),
mX0Loc (0),
mX1Loc (xsz),
mY0Loc (0),
mY1Loc (ysz),
sz (init_sz()),
largef (100*cCRNode::MaxVal),
mCRQ (3*xsz*ysz,sizeof(int),2*xsz*ysz),
mCRH (xsz*ysz/4,(xsz*ysz)*2,4*(xsz+ysz),10*(xsz+ysz)),
mLNodes (new cCRNode [sz]),
mCNodes (AllocCNodes()),
mNoLabel (sz+2),
mNoLabelBorder (sz+9),
mSourceHeight (mNoLabel),
mSinkHeight (0),
mDirZPlus (-1),
mDirZMoins (-1),
mNbDischarge (0),
mNbLift (0),
mNbRelabel (0),
mNLA (false)
{
for (int ed=0 ; ed<eNbEdges ; ed++)
{
if ((xdelta[ed]==0)&&(ydelta[ed]==0)&&(zdelta[ed]==1))
mDirZPlus = ed;
if ((xdelta[ed]==0)&&(ydelta[ed]==0)&&(zdelta[ed]==-1))
mDirZMoins = ed;
}
assert((mDirZPlus!=-1) && (mDirZMoins!=-1));
/** global share crap **/
// printf("--- max flow (%d,%d,%d) s=%d ---\n",mX1Glob,mY1Glob,mZMaxGlob-mZMinGlob,sz);
// printf("--- sizeof(cCRNode) = %d ---\n",sizeof(cCRNode));
// printf("--- total mem required = %dk ---\n",sz*sizeof(cCRNode)/1000);
/** for relabel **/
// printf("--- init ---\n");
if( mLNodes==NULL )
{
printf("Out of Mem! n\n");
ElEXIT(-1,"Cox Roy Out of Memory");
}
mSinkFlow=0;
// printf("--- Go! ---\n");
// Begin Init()
cRoyPt aPU(0,0,0);
for ( aPU.mX=mX0Glob; aPU.mX<mX1Glob ; aPU.mX++)
{
for (aPU.mY=mY0Glob; aPU.mY<mY1Glob ; aPU.mY++)
{
int aZ0 = ZMin(aPU.mX,aPU.mY);
int aZ1 = ZMax(aPU.mX,aPU.mY);
for (aPU.mZ=aZ0 ; aPU.mZ<aZ1 ; aPU.mZ++)
{
cCRNode & aNodU = NodeOfP(aPU);
aNodU.SetExcess(0);
aNodU.SetHeight(ZMaxGlob()-aPU.mZ);
aNodU.SetCurrentEdge(0);
for (int ed=0 ; ed<eNbEdges ; ed++)
{
aNodU.SetResidualFlow(ed,cCRNode::MaxVal);
}
}
}
}
SetFlagEdges(mX0Glob,mY0Glob,mX1Glob,mY1Glob,true);
}