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; } } }