avtIVPSolver::Result
avtIVPNIMRODIntegrator::partial_step(const avtIVPField* field,
double *xin, int iflow, double h, double *xout)
{
double Bval, dummy;
xout[0] = xin[0]; xout[1] = xin[1]; xout[2] = xin[2];
/* Q_i */
if (advance(field, xout, iflow, 0, 0.5*h, NEWTACC))
return avtIVPSolver::UNSPECIFIED_ERROR;
/* P_e */
if (getBfield(field, xout, iflow, 1, &Bval, 0, &dummy))
return avtIVPSolver::UNSPECIFIED_ERROR;
xout[flowtable[iflow][1]] += 0.5*h*Bval;
/* P_i */
if (advance(field, xout, iflow, 1, 0.5*h, NEWTACC))
return avtIVPSolver::UNSPECIFIED_ERROR;
/* Q_e */
if (getBfield(field, xout, iflow, 0, &Bval, 0, &dummy))
return avtIVPSolver::UNSPECIFIED_ERROR;
xout[flowtable[iflow][0]] += 0.5*h*Bval;
return avtIVPSolver::OK;
}
int
avtIVPNIMRODIntegrator::advance(const avtIVPField* field,
double *x, int iflow, int icomp, double h, double xacc)
{
double Bval, Bprime, xold, dx;
int it;
const int ITMAX=15;
xold = x[flowtable[iflow][icomp]];
if (getBfield(field, x, iflow, icomp, &Bval, 0, &Bprime)) return 1;
x[flowtable[iflow][icomp]] += h*Bval; /* Initial guess */
/* Newton iteration algorithm from Numerical Recipes */
for (it=1; it<=ITMAX; it++) {
if (getBfield(field, x, iflow, icomp, &Bval, 1, &Bprime)) return it+1;
dx = (x[flowtable[iflow][icomp]] - h*Bval - xold)/(1.0 - h*Bprime);
x[flowtable[iflow][icomp]] -= dx;
if (fabs(dx) < xacc*h) return 0;
} /* end loop it */
if (fabs(dx) > 1.0e-3*h)
fprintf(stderr,
"Newton method failed to converge in %d iterations (dx=%le, h=%le).\n",
it, dx, h);
return 0;
}
avtIVPSolver::Result
avtIVPNIMRODIntegrator::Step(avtIVPField* field, double t_max,
avtIVPStep* ivpstep)
{
const double direction = sign( 1.0, t_max - t );
h = sign( h, direction );
// do not run past integration end
if( (t + 1.01*h - t_max) * direction > 0.0 )
h = t_max - t;
// stepsize underflow?
if( 0.1*std::abs(h) <= std::abs(t)*epsilon )
return avtIVPSolver::STEPSIZE_UNDERFLOW;
avtIVPSolver::Result res;
avtVector yNew = yCur;
// This call begins the NIMROD code.
res = vpstep(field, yCur, h, yNew);
if( res == avtIVPSolver::OK )
{
ivpstep->resize( 4 );
avtVector yCurCart, yNewCart;
yCurCart[0] = yCur[0] * cos(yCur[1]);
yCurCart[1] = yCur[0] * sin(yCur[1]);
yCurCart[2] = yCur[2];
yNewCart[0] = yNew[0] * cos(yNew[1]);
yNewCart[1] = yNew[0] * sin(yNew[1]);
yNewCart[2] = yNew[2];
(*ivpstep)[0] = yCurCart;
(*ivpstep)[1] = (*ivpstep)[0] + getBfield(field,yCur) * h/3.0;
(*ivpstep)[2] = (*ivpstep)[3] - getBfield(field,yNew) * h/3.0;
(*ivpstep)[3] = yNewCart;
ivpstep->t0 = t;
ivpstep->t1 = t + h;
numStep++;
yCur = yNew;
t = t+h;
}
// Reset the step size on sucessful step.
h = h_max;
return res;
}
World::World(){
drawMowedLawn = true;
memset(lawnMowStatus, 0, sizeof lawnMowStatus);
//printf("%d\n", sizeof bfield);
memset(bfield, 0, sizeof bfield);
imgBfield = Mat(WORLD_SIZE_Y, WORLD_SIZE_X, CV_8UC3, Scalar(0,0,0));
imgWorld = Mat(WORLD_SIZE_Y, WORLD_SIZE_X, CV_8UC3, Scalar(0,0,0));
// perimeter lines coordinates (1/10 meter)
std::vector<point_t> list;
list.push_back( (point_t) {30, 35 } );
list.push_back( (point_t) {50, 15 } );
list.push_back( (point_t) {400, 40 } );
list.push_back( (point_t) {410, 50 } );
list.push_back( (point_t) {420, 90 } );
list.push_back( (point_t) {350, 160 } );
list.push_back( (point_t) {320, 190 } );
list.push_back( (point_t) {210, 250 } );
list.push_back( (point_t) {40, 300 } );
list.push_back( (point_t) {20, 290 } );
list.push_back( (point_t) {30, 230 } );
chgStationX = 35;
chgStationY = 150;
// compute magnetic field (compute distance to perimeter lines)
int x1 = list[list.size()-1].x;
int y1 = list[list.size()-1].y;
// for each perimeter line
for (int i=0; i < list.size(); i++){
int x2 = list[i].x;
int y2 = list[i].y;
int dx = (x2-x1);
int dy = (y2-y1);
int len=(sqrt( dx*dx + dy*dy )); // line length
float phi = atan2(dy,dx); // line angle
// compute magnetic field for points (x,y) around perimeter line
for (int y=-200; y < 200; y++){
for (int x=-100; x < len*2+100-1; x++){
int px= x1 + cos(phi)*x/2 - sin(phi)*y;
int py= y1 + sin(phi)*x/2 + cos(phi)*y;
int xend = max(0, min(len, x/2)); // restrict to line ends
int cx = x1 + cos(phi)*xend; // cx on line
int cy = y1 + sin(phi)*xend; // cy on line
if ((py >= 0) && (py < WORLD_SIZE_Y)
&& (px >=0) && (px < WORLD_SIZE_X)) {
float r = max(0.000001, sqrt( (cx-px)*(cx-px) + (cy-py)*(cy-py) ) ) / 10; // distance to line (meter)
float b=100.0/(2.0*M_PI*r); // field strength
int c = pnpoly(list, px, py);
//if ((y<=0) || (bfield[py][px] < 0)){
if (c == 0){
b=b*-1.0;
bfield[py][px] = min(bfield[py][px], b);
} else bfield[py][px] = max(bfield[py][px], b);
}
}
}
x1=x2;
y1=y2;
}
// draw magnetic field onto image
for (int y=0; y < WORLD_SIZE_Y; y++){
for (int x=0; x < WORLD_SIZE_X; x++) {
float b=30 + 30*sqrt( abs(getBfield(x,y)) );
//b:=10 + bfield[y][x];
int v = min(255, max(0, (int)b));
Vec3b intensity;
if (bfield[y][x] > 0){
intensity.val[0]=255-v;
intensity.val[1]=255-v;
intensity.val[2]=255;
} else {
intensity.val[0]=255;
intensity.val[1]=255-v;
intensity.val[2]=255-v;
}
imgBfield.at<Vec3b>(y, x) = intensity;
}
}
}
