float SHRot::dW(const int k, const int l, const int m, const int n) {
if(abs(m) == l || abs(m) == l-1)
return 0.f;
if(m > 0)
return dP(k, l, m+1, n, 1) + dP(k, l, -m-1, n, -1);
else
return dP(k, l, m-1, n, 1) - dP(k, l, -m+1, n, -1);
}
QImage randomImage(int size, random_engine & rng) {
QImage img(size, size, QImage::Format_ARGB32_Premultiplied);
img.fill(Qt::white);
QPainter p(&img);
p.setRenderHint(QPainter::Antialiasing);
int N = std::uniform_int_distribution<>(25, 200)(rng);
std::uniform_real_distribution<> dP(0, size);
std::uniform_int_distribution<> dC(0, 255);
QPointF pt1(dP(rng), dP(rng));
for (int i = 0; i < N; ++i) {
QColor c(dC(rng), dC(rng), dC(rng));
p.setPen(QPen(c, 3));
QPointF pt2(dP(rng), dP(rng));
p.drawLine(pt1, pt2);
pt1 = pt2;
}
return img;
}
float SHRot::dV(const int k, const int l, const int m, const int n) {
if(m > 1)
return dP(k, l, m-1, n, 1) - dP(k, l, -m+1, n, -1);
else if(m == 1)
return sqrtf(2.f)*dP(k, l, 0, n, 1);
else if(m == 0)
return dP(k, l, 1, n, 1) + dP(k, l, -1, n, -1);
else if(m == -1)
return sqrtf(2.f)*dP(k, l, 0, n, -1);
else
return dP(k, l, -m-1, n, -1) + dP(k, l, m+1, n, 1);
}
//==============================================================================
void
patch_model::
train(const vector<Mat> &images,
const Size psize,
const float var,
const float lambda,
const float mu_init,
const int nsamples,
const bool visi)
{
int N = images.size(),n = psize.width*psize.height;
//compute desired response map
Size wsize = images[0].size();
if((wsize.width < psize.width) || (wsize.height < psize.height)){
cerr << "Invalid image size < patch size!" << endl; throw std::exception();
}
int dx = wsize.width-psize.width,dy = wsize.height-psize.height;
Mat F(dy,dx,CV_32F);
for(int y = 0; y < dy; y++){ float vy = (dy-1)/2 - y;
for(int x = 0; x < dx; x++){ float vx = (dx-1)/2 - x;
F.fl(y,x) = exp(-0.5*(vx*vx+vy*vy)/var);
}
}
normalize(F,F,0,1,NORM_MINMAX);
//allocate memory
Mat I(wsize.height,wsize.width,CV_32F);
Mat dP(psize.height,psize.width,CV_32F);
Mat O = Mat::ones(psize.height,psize.width,CV_32F)/n;
P = Mat::zeros(psize.height,psize.width,CV_32F);
//optimise using stochastic gradient descent
RNG rn(getTickCount()); double mu=mu_init,step=pow(1e-8/mu_init,1.0/nsamples);
for(int sample = 0; sample < nsamples; sample++){ int i = rn.uniform(0,N);
I = this->convert_image(images[i]); dP = 0.0;
for(int y = 0; y < dy; y++){
for(int x = 0; x < dx; x++){
Mat Wi = I(Rect(x,y,psize.width,psize.height)).clone();
Wi -= Wi.dot(O); normalize(Wi,Wi);
dP += (F.fl(y,x) - P.dot(Wi))*Wi;
}
}
P += mu*(dP - lambda*P); mu *= step;
if(visi){
Mat R; matchTemplate(I,P,R,CV_TM_CCOEFF_NORMED);
Mat PP; normalize(P,PP,0,1,NORM_MINMAX);
normalize(dP,dP,0,1,NORM_MINMAX);
normalize(R,R,0,1,NORM_MINMAX);
imshow("P",PP); imshow("dP",dP); imshow("R",R);
if(waitKey(10) == 27)break;
}
}return;
}
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
// double *param0 = mxGetPr(ssGetSFcnParam(S,0));
int i, j, k;
double u[N_INPUTS];
// pointers to 'input structure'
double *alf = u, *mu = alf+NS, *wt = mu+NI;
// double a[NS], b[NI], Ko_b[NS], K_b[NS];
double *P__ = x;
double *dP__ = dx;
double dp;
double *P_[NS], *dP_[NS]; // array of pointers for lower triangular P, dP
double A_[NS2], A_BKo_[NS2];
double B_[NS*NI], Ko_[NI*NS];
double Q_[NS2];
/* index A & P with 1:NS (NOT 0:NS-1!) */
#define A(i,j) A_[((i)-1) + NS*((j)-1)]
#define Q(i,j) Q_[((i)-1) + NS*((j)-1)]
#define P(i,j) (( (j<=i) ? P_[i-1][j-1] : P_[j-1][i-1] )) // low tri
#define dP(i,j) ( dP_[i-1][j-1] ) // *** i <= j *** REQUIRED!
#define B(i,j) B_[ ((i)-1) + ((j)-1)*(NS) ] // columnwise
#define Ko(i,j) Ko_[ ((i)-1)*NS + ((j)-1) ] // rowwise
// set up lower triangular structure for P and dP
j = 0;
for (i=0; i<NS; i++) {
P_[i] = P__ + j;
dP_[i] = dP__ + j;
j += i+1;
}
/* copy inputs to u[] to ensure contiguity ... and easy access*/
for (i=0; i<N_INPUTS; i++)
u[i] = *uPtrs[i];
/* prontoTK spec:
dynamics(x,u,wt,ders, dx,y, fxu_x_,fxu_u_, q,q_fxu_x_x_,q_fxu_x_u_,q_fxu_u_u_);
ders: dx(1),y(2), A(4),B(8), Q(16), S(32), R(64)
cost(x,u,wlt,ders, lxu, lxu_x_,lxu_u_, lxu_x_x_,lxu_x_u_,lxu_u_u_);
ders: lxu(1), a(2),b(4), Q(8), S(16), R(32)
*/
// get A of linearization about (alf(t),mu(t))
// get A(4)
dynamics(alf,mu,wt, 4, NULL,NULL, A_,NULL, NULL, NULL,NULL,NULL);
/* set up Q matrix --- start with zero */
for (i=0; i<NS2; i++){
Q_[i] = 0.0;
}
/* add diagonal cost fcn terms */
for (i=1; i<=NS; i++){
Q(i,i) = Qr[i-1];
}
optBK(B_, Ko_, x, u);
/* only *lower triangle* of dP */
for (i=1; i<=NS; i++) {
for (j=1; j<=i; j++) {
dp = Q(i,j);
for (k=1; k<=NS; k++) {
dp += A(k,i)*P(k,j) + P(i,k)*A(k,j); // A^T P + P A
}
for (k=1; k<=NI; k++) {
dp -= Ko(k,i)*Rr[k-1]*Ko(k,j); // -P B R^-1 B^T P = -Ko^T R Ko
}
dP(i,j) /* = dP(j,i) */ = dp;
}
}
#undef B
#undef K
#undef Ko
#undef dP
#undef P
#undef A
}
int MaxwellCorrectionGadget::
process(GadgetContainerMessage<ISMRMRD::ImageHeader>* m1,
GadgetContainerMessage< hoNDArray< std::complex<float> > >* m2)
{
if (maxwell_coefficients_present_) {
//GDEBUG("Got coefficients\n");
int Nx = m2->getObjectPtr()->get_size(0);
int Ny = m2->getObjectPtr()->get_size(1);
int Nz = m2->getObjectPtr()->get_size(2);
float dx = m1->getObjectPtr()->field_of_view[0] / Nx;
float dy = m1->getObjectPtr()->field_of_view[1] / Ny;
float dz = m1->getObjectPtr()->field_of_view[2] / Nz;
/*
GDEBUG("Nx = %d, Ny = %d, Nz = %d\n", Nx, Ny, Nz);
GDEBUG("dx = %f, dy = %f, dz = %f\n", dx, dy, dz);
GDEBUG("img_pos_x = %f, img_pos_y = %f, img_pos_z = %f\n", m1->getObjectPtr()->position[0], m1->getObjectPtr()->position[1], m1->getObjectPtr()->position[2]);
*/
std::vector<float> dR(3,0);
std::vector<float> dP(3,0);
std::vector<float> dS(3,0);
std::vector<float> p(3,0);
for (int z = 0; z < Nz; z++) {
for (int y = 0; y < Ny; y++) {
for (int x = 0; x < Nx; x++) {
dR[0] = (x-Nx/2+0.5) * dx * m1->getObjectPtr()->read_dir[0];
dR[1] = (x-Nx/2+0.5) * dx * m1->getObjectPtr()->read_dir[1];
dR[2] = (x-Nx/2+0.5) * dx * m1->getObjectPtr()->read_dir[2];
dP[0] = (y-Ny/2+0.5) * dy * m1->getObjectPtr()->phase_dir[0];
dP[1] = (y-Ny/2+0.5) * dy * m1->getObjectPtr()->phase_dir[1];
dP[2] = (y-Ny/2+0.5) * dy * m1->getObjectPtr()->phase_dir[2];
if (Nz > 1) {
dS[0] = (z-Nz/2+0.5) * dz * m1->getObjectPtr()->slice_dir[0];
dS[1] = (z-Nz/2+0.5) * dz * m1->getObjectPtr()->slice_dir[1];
dS[2] = (z-Nz/2+0.5) * dz * m1->getObjectPtr()->slice_dir[2];
}
p[0] = m1->getObjectPtr()->position[0] + dP[0] + dR[0] + dS[0];
p[1] = m1->getObjectPtr()->position[1] + dP[1] + dR[1] + dS[1];
p[2] = m1->getObjectPtr()->position[2] + dP[2] + dR[2] + dS[2];
//Convert to centimeters
p[0] = p[0]/1000.0;
p[1] = p[1]/1000.0;
p[2] = p[2]/1000.0;
float delta_phi = maxwell_coefficients_[0]*p[2]*p[2] +
maxwell_coefficients_[1]*(p[0]*p[0] + p[1]*p[1]) +
maxwell_coefficients_[2]*p[0]*p[2] +
maxwell_coefficients_[3]*p[1]*p[2];
long index = z*Ny*Nx+y*Nx+x;
std::complex<float>* data_ptr = m2->getObjectPtr()->get_data_ptr();
std::complex<float> correction = std::polar(1.0f,static_cast<float>(2*M_PI*delta_phi));
data_ptr[index] *= correction;
}
}
}
}
if (this->next()->putq(m1) < 0) {
GDEBUG("Unable to put data on next Gadgets Q\n");
return GADGET_FAIL;
}
return GADGET_OK;
}
bool QMCSHLinearOptimize::run()
{
start();
//size of matrix
numParams = optTarget->NumParams();
N = numParams + 1;
// initialize our parameters
vector<RealType> currentOvlp(N,0);
vector<RealType> currentHOvlp(N,0);
vector<RealType> currentParameters(numParams,0);
RealType E_avg(0);
vector<RealType> bestParameters(currentParameters);
optdir.resize(numParams,0);
optparm.resize(numParams,0);
for (int i=0; i<numParams; i++) optparm[i] = currentParameters[i] = optTarget->Params(i);
bool acceptedOneMove(false);
int tooManyTries(20);
int failedTries(0);
RealType lastCost(0);
RealType startCost(0);
startCost = lastCost = optTarget->Cost(false);
app_log()<<"Starting cost: "<<startCost<<endl;
Matrix<RealType> OM;
OM.resize(N,N);
dmcEngine->fillVectors(currentOvlp,currentHOvlp,E_avg,OM);
for (int i=0; i<numParams; i++) optdir[i]=currentOvlp[i];
std::vector<RealType> dP(N,1);
for (int i=0; i<numParams; i++) dP[i+1]=optdir[i];
Lambda = getNonLinearRescale(dP,OM);
app_log()<<"rescaling factor :"<<Lambda<<endl;
RealType bigOptVec(std::abs(optdir[0]));
for (int i=1; i<numParams; i++) bigOptVec =std::max(std::abs(optdir[i]),bigOptVec);
// app_log()<<"currentOvlp"<<endl;
// for (int i=0; i<numParams; i++)
// app_log()<<optdir[i]<<" ";
// app_log()<<endl;
//
// app_log()<<"optparam"<<endl;
// for (int i=0; i<numParams; i++)
// app_log()<<optparm[i]<<" ";
// app_log()<<endl;
if (MinMethod=="rescale")
{
if (bigOptVec*std::abs(Lambda)>bigChange)
app_log()<<" Failed Step. Largest LM parameter change:"<<bigOptVec*std::abs(Lambda)<<endl;
else
{
for (int i=0; i<numParams; i++) bestParameters[i] = optTarget->Params(i) = currentParameters[i] + Lambda*optdir[i];
acceptedOneMove = true;
}
}
else if (MinMethod=="overlap")
{
orthoScale(optdir,OM);
if (bigOptVec>bigChange)
app_log()<<" Failed Step. Largest LM parameter change:"<<bigOptVec<<endl;
else
{
for (int i=0; i<numParams; i++) bestParameters[i] = optTarget->Params(i) = currentParameters[i] + optdir[i];
acceptedOneMove = true;
}
}
else if (MinMethod=="average")
{
for (int i=0; i<numParams; i++) bestParameters[i] = optTarget->Params(i) = currentParameters[i] + currentHOvlp[i];
acceptedOneMove = true;
}
else
{
TOL = param_tol/bigOptVec;
AbsFuncTol=true;
largeQuarticStep=bigChange/bigOptVec;
quadstep = stepsize*Lambda; //bigOptVec;
// initial guess for line min bracketing
LambdaMax = quadstep;
myTimers[3]->start();
if (MinMethod=="quartic")
{
int npts(7);
lineoptimization3(npts,startCost);
}
else lineoptimization2();
myTimers[3]->stop();
RealType biggestParameterChange = bigOptVec*std::abs(Lambda);
if (biggestParameterChange>bigChange)
{
app_log()<<" Failed Step. Largest LM parameter change:"<<biggestParameterChange<<endl;
for (int i=0; i<numParams; i++) optTarget->Params(i) = bestParameters[i] = currentParameters[i];
}
else
{
for (int i=0; i<numParams; i++) optTarget->Params(i) = optparm[i] + Lambda * optdir[i];
lastCost = optTarget->Cost(false);
app_log()<<" Costs: "<<startCost<<" "<<lastCost<<endl;
app_log()<<" Optimal rescaling factor :"<<Lambda<<endl;
if (lastCost<startCost)
acceptedOneMove = true;
else
{
for (int i=0; i<numParams; i++) optTarget->Params(i) = currentParameters[i];
app_log()<<" Failed Step. Cost increase "<<endl;
}
}
}
// if (acceptedOneMove)
// for (int i=0; i<numParams; i++) optTarget->Params(i) = bestParameters[i];
// else
// for (int i=0; i<numParams; i++) optTarget->Params(i) = currentParameters[i];
// if (W.getActiveWalkers()>NumOfVMCWalkers)
// {
// W.destroyWalkers(W.getActiveWalkers()-NumOfVMCWalkers);
// app_log() << " QMCLinearOptimize::generateSamples removed walkers." << endl;
// app_log() << " Number of Walkers per node " << W.getActiveWalkers() << endl;
// }
finish();
return (optTarget->getReportCounter() > 0);
}
float SHRot::dU(const int k, const int l, const int m, const int n) {
if(abs(m) == l)
return 0.f;
else
return dP(k, l, m, n, 0);
}
void
mlrate(double mean, FILE *fdiag)
{
double maxchange; /* The max change of one turn. */
size_t pcount, gcount, rcount; /* Player, game, and removed counters. */
size_t wcount, lcount; /* Win/loss counters. */
double wsum, lsum; /* Weighted sums. */
size_t globturns = 0; /* Counts the turns of the outer loop. */
size_t hcount[10]; /* Handicap game counters. */
player_t p; /* A player. */
piter_t piter; /* An iterator over players. */
char *pflags;
assert(0.0 < mean && mean < RANK_MAXIMUM);
pflags = (char *)malloc(player_count());
if (pflags == NULL)
errex("malloc(%lu) failed", player_count());
memset(pflags, 0, player_count());
/* Assign a start rating for every player. */
player_start(&piter);
while ((p = player_next(&piter)) != NO_PLAYER)
if (!player_get_rated(p) && !player_get_ignore(p))
{
player_set_rank(p, mean);
player_set_rated(p, 1);
}
/* Remove players with no wins or no losses against other rated
** players; then again and again, until no more can be removed.
** While we're at it, count some statistics as well.
*/
do {
pcount = gcount = rcount = 0;
hcount[0] = hcount[1] = hcount[2] = hcount[3] = hcount[4] =
hcount[5] = hcount[6] = hcount[7] = hcount[8] = hcount[9] = 0;
player_start(&piter);
while ((p = player_next(&piter)) != NO_PLAYER)
if (player_get_rated(p) && !player_get_ignore(p))
{
game_t g;
giter_t giter;
unsigned oppcount = 0;
wcount = lcount = 0;
wsum = lsum = 0.0;
player_games_start(p, &giter);
while ((g = player_games_next(&giter)) != NO_GAME)
if (game_weight(g) > 0.0)
{
double a = game_advantage(g);
if (a >= 10.0)
hcount[0] += 1;
else if (a >= 1.0)
hcount[(unsigned)floor(a)] += 1;
if (p == game_winner(g) && player_get_rated(game_loser(g)))
{
wcount += 1;
wsum += game_weight(g);
if (!pflags[game_loser(g)])
{
pflags[game_loser(g)] = 1;
oppcount += 1;
}
}
else if (p == game_loser(g) && player_get_rated(game_winner(g)))
{
lcount += 1;
lsum += game_weight(g);
if (!pflags[game_winner(g)])
{
pflags[game_winner(g)] = 1;
oppcount += 1;
}
}
else
game_set_weight(g, 0.0);
}
else
game_set_weight(g, 0.0);
if (wsum < 0.25 || lsum < 0.25 || oppcount < 3)
{
player_set_rated(p, 0);
rcount += 1;
}
else
{
pcount += 1;
gcount += wcount + lcount;
}
player_set_ratedgames(p, wcount, lcount);
player_set_wratedgames(p, wsum, lsum);
/* Clear flags. */
player_games_start(p, &giter);
while ((g = player_games_next(&giter)) != NO_GAME)
pflags[game_loser(g)] = pflags[game_winner(g)] = 0;
}
} while (rcount > 0);
player_gc_games();
if (fdiag)
{
int i;
fprintf(fdiag, "\nRemaining:\n%6lu players\n%6lu games\n\n",
(unsigned long)pcount, (unsigned long)gcount/2);
for (i = 1 ; i <= 9 ; i++)
fprintf(fdiag, "Advantage %2d: %5lu games\n",
i, (unsigned long)hcount[i]);
fprintf(fdiag, "Advantage >=10: %5lu games\n\n",
(unsigned long)hcount[0]);
}
if (pcount == 0 || gcount == 0)
errex("No player or no games");
/*
** The outer loop.
*/
do { /* while (maxchange > CHANGE_LIMIT && globturns < GLOBAL_TURNS_MAX); */
int maxp = 0;
pcount = 0;
maxchange = 0.0;
globturns += 1;
/*
** Loop over all players.
*/
player_start(&piter);
while ((p = player_next(&piter)) != NO_PLAYER)
{
/*
** We use bisection to find the root of the derivative (the maximum).
*/
irank_t r, oldrank;
irank_t ileft = RANK_MINIMUM, iright = RANK_MAXIMUM;
if (!player_get_rated(p) || player_get_ignore(p))
continue;
/*
** Inner (bisection) loop.
*/
pcount += 1;
r = oldrank = player_get_rank(p);
do { /* while (iright - ileft > CLOSE_ENOUGH); */
game_t g;
double sum = 0.0;
giter_t giter;
player_games_start(p, &giter);
while ((g = player_games_next(&giter)) != NO_GAME)
{
player_t opp;
double diff;
if (p == game_winner(g))
{
opp = game_loser(g);
if (player_get_rated(opp))
{
diff = RANK_DIFF(r, player_get_rank(opp))
+ game_advantage(g);
sum += dP(diff, 1) * game_weight(g);
}
}
else
{
opp = game_winner(g);
if (player_get_rated(opp))
{
diff = RANK_DIFF(r, player_get_rank(opp))
- game_advantage(g);
sum += dP(diff, 0) * game_weight(g);
}
}
}
if (sum > 0.0)
iright = r; /* Root's somewhere to the left. */
else
ileft = r; /* Root's somewhere to the right. */
r = (iright + ileft)/2;
} while (iright - ileft > CLOSE_ENOUGH);
if (r > oldrank)
{
if (r - oldrank > maxchange)
{
maxchange = r - oldrank;
maxp = p;
}
}
else
{
if (oldrank - r > maxchange)
{
maxchange = oldrank - r;
maxp = p;
}
}
player_set_rank(p, r);
} /* while ((p = player_next())) */
#ifdef MAXP
fprintf(stderr, "\n--- Maxp: %s rank=%g ww=%g wl=%g w=%u l=%u rg=%u\n",
player_get_name(maxp),
player_get_rank(maxp),
player_get_wwins(maxp),
player_get_wlosses(maxp),
player_get_wins(maxp),
player_get_losses(maxp),
player_get_ratedgames(maxp));
#endif
if (globturns > 100)
circular_check(maxp);
if (fdiag)
{
fprintf(fdiag, " %3lu: %6.3f", (unsigned long)globturns, maxchange);
if (globturns % 5)
fflush(fdiag);
else
fputc('\n', fdiag);
}
} while (maxchange > CHANGE_LIMIT && globturns < GLOBAL_TURNS_MAX);
if (fdiag)
{
if (globturns % 5)
fputc('\n', fdiag);
fputc('\n', fdiag);
}
if (globturns == GLOBAL_TURNS_MAX)
errex("Aborted after maximum %u turns\n", GLOBAL_TURNS_MAX);
if (pflags != NULL)
free(pflags);
}
void ExampleFunction::evaluateFG(const vector<double> &x, double &f, vector<double> &g)
{
if(count < 0 ){count--;if(count==-3)count = 0;
f = 0;
for(unsigned i = 0;i<2*_placement.numModules();++i)
g[i] = 0;
unsigned num = _placement.numModules();
double dx,dy,absdx,absdy;
double Px,Py;
double dPx,dPy;
double D = 0;
double a_x, b_x;
double a_y, b_y;
double binNum = 25.0;
double binWidth = (_placement.boundryRight() - _placement.boundryLeft()) / sqrt(binNum);
double binHeight = (_placement.boundryTop() - _placement.boundryBottom()) / sqrt(binNum);
double M = 800;//0*(binWidth*binHeight - 0);
vector<double> binCenterX,binCenterY,binCenterZ;
vector<double> dP(2*num,0);
vector<double> P(2*num,0);
for(int i = 0 ; i < sqrt(binNum) ; i++){
binCenterX.push_back(_placement.boundryLeft() + (2*i+1)*((_placement.boundryRight() - _placement.boundryLeft())/ (sqrt(binNum)*2)));
binCenterY.push_back(_placement.boundryTop() - (2*i+1)*((_placement.boundryTop() - _placement.boundryBottom())/ (sqrt(binNum)*2)));
}
for(unsigned y = 0 ; y < sqrt(binNum) ; y++){
for(unsigned i = 0 ; i < sqrt(binNum) ; i++){
D = 0;
for(unsigned j = 0 ; j < num ; j++){
// x coordinate
a_x = 4/((_placement.module(j).width() + 2*binWidth)*(_placement.module(j).width() + 4*binWidth));
b_x = 2/(binWidth*(_placement.module(j).width() + 4*binWidth));
dx = x[j] - binCenterX[i];
absdx = abs(dx);
if(absdx >= 0 && absdx <= (_placement.module(j).width()/2 + binWidth) ){
Px = 1 - a_x*dx*dx;
dPx = -2*a_x*dx;
}
else if (absdx >= (_placement.module(j).width()/2 + 2*binWidth)){
Px = 0;
dPx = 0;
}
else{
Px = b_x*(absdx - _placement.module(j).width()/2 - 2*binWidth)*(absdx - _placement.module(j).width()/2 - 2*binWidth);
if(dx>0)
dPx = 2*b_x*(dx - _placement.module(j).width()/2 - 2*binWidth);
else
dPx = 2*b_x*(dx + _placement.module(j).width()/2 + 2*binWidth);
}
// y coordinate
a_y = 4/((_placement.module(j).height() + 2*binHeight)*(_placement.module(j).height() + 4*binHeight));
b_y = 2/(binHeight*(_placement.module(j).height() + 4*binHeight));
dy = x[j+num] - binCenterY[y];
absdy = abs(dy);
if(absdy >= 0 && absdy <= (_placement.module(j).height()/2 + binHeight) ){
Py = 1 - a_y*dy*dy;
dPy = -2*a_y*dy;
}
else if (absdy >= (_placement.module(j).height()/2 + 2*binHeight)){
Py = 0;
dPy = 0;
}
else{
Py = b_y*(absdy - _placement.module(j).height()/2 - 2*binHeight)*(absdy - _placement.module(j).height()/2 - 2*binHeight);
if(dy>0)
dPy = 2*b_y*(dy - _placement.module(j).height()/2 - 2*binHeight);
else
dPy = 2*b_y*(dy + _placement.module(j).height()/2 + 2*binHeight);
}
dP[j] = dPx;
dP[j+num] = dPy;
P[j] = Px;
P[j+num] = Py;
D += Px*Py;
}
f += (D - M)*(D - M);
for(unsigned j = 0 ; j < num ; j++){
g[j] += 2*(D-M)*dP[j]*P[j+num];
g[j+num] += 2*(D-M)*dP[j+num]*P[j];
if( x[j]+2*_placement.module(j).width() < _placement.boundryLeft() || x[j]-2*_placement.module(j).width() > _placement.boundryRight() )
g[j] = 0;
if( x[j+num]+2*_placement.module(j).height() < _placement.boundryBottom() || x[j+num]-2*_placement.module(j).height() > _placement.boundryTop() )
g[j+num] = 0;
}
}
}
}
///////////////////////////////////////////////////////////////////////
else{if(count==0)count--;
f = 0;
unsigned num = _placement.numModules();
double r = 0.01*(_placement.boundryRight() - _placement.boundryLeft());
for(unsigned i = 0;i<2*num;++i)
g[i] = 0;
for(unsigned i = 0 ; i<_placement.numNets();++i){
double f_1=0.0,f_2=0.0,f_3=0.0,f_4=0.0;
vector<unsigned> moduleID;
Net& net = _placement.net(i);
for(unsigned j = 0; j<net.numPins();++j){
moduleID.push_back(net.pin(j).moduleId());
}
for(unsigned k = 0; k<moduleID.size();++k){
if(_placement.module(moduleID[k]).isFixed()){
Module& module = _placement.module(moduleID[k]);
f_1 = f_1 + exp( (module.centerX()+net.pin(k).xOffset()) /r);
f_2 = f_2 + exp((-1*(module.centerX()+net.pin(k).xOffset()))/r);
f_3 = f_3 + exp((module.centerY()+net.pin(k).yOffset())/r);
f_4 = f_4 + exp((-1*(module.centerY()+net.pin(k).yOffset()))/r);
}
else{
f_1 = f_1 + exp((x[moduleID[k]]+net.pin(k).xOffset())/r);
f_2 = f_2 + exp((-1*(x[moduleID[k]]+net.pin(k).xOffset()))/r);
f_3 = f_3 + exp((x[moduleID[k]+num]+net.pin(k).yOffset())/r);
f_4 = f_4 + exp((-1*(x[moduleID[k]+num])+net.pin(k).xOffset())/r);
}
}
f = f + log(f_1) + log(f_2) + log(f_3) + log(f_4) ;
for(unsigned k=0; k<moduleID.size();++k){
if(_placement.module(moduleID[k]).isFixed()==false){
g[moduleID[k]] += ( (exp((x[moduleID[k]]+net.pin(k).xOffset())/r))*(1/f_1) + (exp((-1*(x[moduleID[k]]+net.pin(k).xOffset()))/r)*(-1))*(1/f_2) );
g[moduleID[k]+num] += ( (exp((x[moduleID[k]+num]+net.pin(k).yOffset())/r))*(1/f_3) + (exp((-1*(x[moduleID[k]+num])+net.pin(k).xOffset())/r)*(-1))*(1/f_4) );
}
}
}
f = f * r;
}
}
double SegSegDist(const hrp::Vector3& u0, const hrp::Vector3& u1,
const hrp::Vector3& v0, const hrp::Vector3& v1)
{
hrp::Vector3 u(u1 - u0);
hrp::Vector3 v(v1 - v0);
hrp::Vector3 w(u0 - v0);
double a = u.dot(u); // always >= 0
double b = u.dot(v);
double c = v.dot(v); // always >= 0
double d = u.dot(w);
double e = v.dot(w);
double D = a*c - b*b; // always >= 0
double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
#define EPS 1e-8
if (D < EPS) { // the lines are almost parallel
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else {
sN = -d;
sD = a;
}
}
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (fabsf(sN) < EPS ? 0.0f : sN / sD);
tc = (fabsf(tN) < EPS ? 0.0f : tN / tD);
hrp::Vector3 cp0(u0 + sc * u);
hrp::Vector3 cp1(v0 + tc * v);
// get the difference of the two closest points
hrp::Vector3 dP(cp0 - cp1);
return dP.norm(); // return the closest distance
}
