void GraphComparator::_paintWay(cv::Mat& mat, ConstOsmMapPtr map, shared_ptr<Way> way, double friction,
double startCost, double endCost)
{
LocationOfPoint lop(map, way);
double length = ElementConverter(map).convertToLineString(way)->getLength();
// v is the distance along the way in meters
for (double v = 0.0; v <= length; v += _pixelSize / 2.0)
{
double cost = std::min(startCost + v * friction, endCost + (length - v) * friction);
Coordinate c = lop.locate(v);
int x = (c.x - _projectedBounds.MinX) / _pixelSize;
int y = _height - (c.y - _projectedBounds.MinY) / _pixelSize;
float* row = mat.ptr<float>(y);
if (row[x] >= 0.0)
{
row[x] = std::min((float)cost, row[x]);
}
else
{
row[x] = cost;
}
}
}
void mgfas(double **u, int n, int maxcyc)
{
double anorm2(double **a, int n);
void copy(double **aout, double **ain, int n);
void interp(double **uf, double **uc, int nf);
void lop(double **out, double **u, int n);
void matadd(double **a, double **b, double **c, int n);
void matsub(double **a, double **b, double **c, int n);
void relax2(double **u, double **rhs, int n);
void rstrct(double **uc, double **uf, int nc);
void slvsm2(double **u, double **rhs);
unsigned int j,jcycle,jj,jm1,jpost,jpre,nf,ng=0,ngrid,nn;
double **irho[NGMAX+1],**irhs[NGMAX+1],**itau[NGMAX+1],
**itemp[NGMAX+1],**iu[NGMAX+1];
double res,trerr;
nn=n;
while (nn >>= 1) ng++;
if (n != 1+(1L << ng)) nrerror("n-1 must be a power of 2 in mgfas.");
if (ng > NGMAX) nrerror("increase NGMAX in mglin.");
nn=n/2+1;
ngrid=ng-1;
irho[ngrid]=dmatrix(1,nn,1,nn);
rstrct(irho[ngrid],u,nn);
while (nn > 3) {
nn=nn/2+1;
irho[--ngrid]=dmatrix(1,nn,1,nn);
rstrct(irho[ngrid],irho[ngrid+1],nn);
}
nn=3;
iu[1]=dmatrix(1,nn,1,nn);
irhs[1]=dmatrix(1,nn,1,nn);
itau[1]=dmatrix(1,nn,1,nn);
itemp[1]=dmatrix(1,nn,1,nn);
slvsm2(iu[1],irho[1]);
free_dmatrix(irho[1],1,nn,1,nn);
ngrid=ng;
for (j=2;j<=ngrid;j++) {
nn=2*nn-1;
iu[j]=dmatrix(1,nn,1,nn);
irhs[j]=dmatrix(1,nn,1,nn);
itau[j]=dmatrix(1,nn,1,nn);
itemp[j]=dmatrix(1,nn,1,nn);
interp(iu[j],iu[j-1],nn);
copy(irhs[j],(j != ngrid ? irho[j] : u),nn);
for (jcycle=1;jcycle<=maxcyc;jcycle++) {
nf=nn;
for (jj=j;jj>=2;jj--) {
for (jpre=1;jpre<=NPRE;jpre++)
relax2(iu[jj],irhs[jj],nf);
lop(itemp[jj],iu[jj],nf);
nf=nf/2+1;
jm1=jj-1;
rstrct(itemp[jm1],itemp[jj],nf);
rstrct(iu[jm1],iu[jj],nf);
lop(itau[jm1],iu[jm1],nf);
matsub(itau[jm1],itemp[jm1],itau[jm1],nf);
if (jj == j)
trerr=ALPHA*anorm2(itau[jm1],nf);
rstrct(irhs[jm1],irhs[jj],nf);
matadd(irhs[jm1],itau[jm1],irhs[jm1],nf);
}
slvsm2(iu[1],irhs[1]);
nf=3;
for (jj=2;jj<=j;jj++) {
jm1=jj-1;
rstrct(itemp[jm1],iu[jj],nf);
matsub(iu[jm1],itemp[jm1],itemp[jm1],nf);
nf=2*nf-1;
interp(itau[jj],itemp[jm1],nf);
matadd(iu[jj],itau[jj],iu[jj],nf);
for (jpost=1;jpost<=NPOST;jpost++)
relax2(iu[jj],irhs[jj],nf);
}
lop(itemp[j],iu[j],nf);
matsub(itemp[j],irhs[j],itemp[j],nf);
res=anorm2(itemp[j],nf);
if (res < trerr) break;
}
}
copy(u,iu[ngrid],n);
for (nn=n,j=ng;j>=1;j--,nn=nn/2+1) {
free_dmatrix(itemp[j],1,nn,1,nn);
free_dmatrix(itau[j],1,nn,1,nn);
free_dmatrix(irhs[j],1,nn,1,nn);
free_dmatrix(iu[j],1,nn,1,nn);
if (j != ng && j != 1) free_dmatrix(irho[j],1,nn,1,nn);
}
}
void Assem_x86::assemInst( const string &name,const string &lhs,const string &rhs ) {
//parse operands
Operand lop( lhs ),rop( rhs );
lop.parse();
rop.parse();
//find instruction
int cc=-1;
Inst *inst=0;
//kludge for condition code instructions...
if( name[0]=='j' ) {
if( (cc=findCC(name.substr(1)))>=0 ) {
static Inst jCC= { "jCC",IMM,NONE,RW_RD|PLUSCC,"\x2\x0F\x80" };
inst=&jCC;
}
} else if( name[0]=='s' && name.substr( 0,3 )=="set" ) {
if( (cc=findCC(name.substr(3)))>=0 ) {
static Inst setCC= { "setne",R_M8,NONE,_2|PLUSCC,"\x2\x0F\x90" };
inst=&setCC;
}
}
if( inst ) {
if( !(lop.mode&inst->lmode) || !(rop.mode&inst->rmode) ) throw Ex( "illegal addressing mode" );
} else {
InstIter it=instMap.find( name );
if( it==instMap.end() ) throw Ex( "unrecognized instruction" );
inst=it->second;
for(;;) {
if( (lop.mode&inst->lmode) && (rop.mode&inst->rmode) ) break;
if( (++inst)->name ) throw Ex( "illegal addressing mode" );
}
}
//16/32 bit modifier - NOP for now
if( inst->flags & (O16|O32) ) {}
int k,n=inst->bytes[0];
for( k=1; k<n; ++k ) emit( inst->bytes[k] );
if( inst->flags&PLUSREG ) emit( inst->bytes[k]+lop.reg );
else if( inst->flags&PLUSCC ) emit( inst->bytes[k]+cc );
else emit( inst->bytes[k] );
if( inst->flags&(_0|_1|_2|_3|_4|_5|_6|_7|_R ) ) {
//find the memop;
const Operand &mop=
(inst->rmode&(MEM|MEM8|MEM16|MEM32|R_M|R_M8|R_M16|R_M32))?rop:lop;
//find the spare field value.
int rm=0;
switch( inst->flags&(_0|_1|_2|_3|_4|_5|_6|_7|_R ) ) {
case _0:
rm=0;
break;
case _1:
rm=1;
break;
case _2:
rm=2;
break;
case _3:
rm=3;
break;
case _4:
rm=4;
break;
case _5:
rm=5;
break;
case _6:
rm=6;
break;
case _7:
rm=7;
break;
case _R:
rm=(inst->rmode&(REG8|REG16|REG32))?rop.reg:lop.reg;
break;
}
rm<<=3;
if( mop.mode & REG ) { //reg
emit( 0xc0|rm|mop.reg );
} else if( mop.baseReg>=0 ) { //base, index?
int mod=mop.offset ? 0x40 : 0x00;
if( mop.baseLabel.size() || mop.offset<-128 || mop.offset>127 ) mod=0x80;
if( mop.baseReg==5 && !mod ) mod=0x40;
if( mop.indexReg>=0 ) { //base, index!
emit( mod|rm|4 );
emit( (mop.shift<<6)|(mop.indexReg<<3)|mop.baseReg );
} else { //base, no index!
if( mop.baseReg!=4 ) emit( mod|rm|mop.baseReg);
else {
emit( mod|rm|4 );
emit( (4<<3)|mop.baseReg );
}
}
if( (mod&0xc0)==0x40 ) emit( mop.offset );
else if( (mod&0xc0)==0x80 ) {
//reloc
a_reloc( mop.baseLabel );
emitd( mop.offset );
}
} else if( mop.indexReg>=0 ) { //index, no base!
emit( rm|4 );
emit( (mop.shift<<6)|(mop.indexReg<<3)|5 );
//reloc
a_reloc( mop.baseLabel );
emitd( mop.offset );
} else { //[disp]
emit( rm|5 );
//reloc
a_reloc( mop.baseLabel );
emitd( mop.offset );
}
}
if( inst->flags&RW_RD ) {
r_reloc( lop.immLabel );
emitd( lop.imm-4 );
}
if( inst->flags&IB ) {
if( lop.mode&IMM ) emitImm( lop,1 );
else emitImm( rop,1 );
} else if( inst->flags&IW ) {
if( lop.mode&IMM ) emitImm( lop,2 );
else emitImm( rop,2 );
} else if( inst->flags&ID ) {
if( lop.mode&IMM ) emitImm( lop,4 );
else emitImm( rop,4 );
}
}