void polysolve(double hstep, bool listmodel) { double h, rpmw[4], w1, w2, t; gsl_odeiv2_system sys = { diffrpmw, NULL, 4, NULL }; gsl_odeiv2_driver *drv; int stat; if (npol <= 0.5 || mpol <= -1.0) error("%s: illegal value for n or m\n", getprog()); if (npol >= 3*mpol + 5) error("%s: model would have infinite radius\n", getprog()); Kprime = K * (mpol+1) * pow(PI, -1.5) * pow(2.0, - (mpol+1.5)) * exp(lgamma(mpol+npol+1) - lgamma(mpol+2) - lgamma(npol-0.5)); h = hstep; do { drv = gsl_odeiv2_driver_alloc_y_new(&sys, gsl_odeiv2_step_rk4, h, 1.0, 1.0); rpmw[0] = 0.0; // initialize radius rpmw[1] = PHI0; // initialize potential rpmw[2] = 0.0; // initialize enclosed mass rpmw[3] = 0.0; // initialize binding energy nstep = 0; storestep(rpmw); asmpstep(rpmw, h); // use asymp. approximation storestep(rpmw); while (rpmw[1] < 0.0) { // while potential is neg. stat = gsl_odeiv2_driver_apply_fixed_step(drv, &t, h, 1, rpmw); if (stat) eprintf("[%s.polysolve: stat = %d]\n", getprog(), stat); storestep(rpmw); } fixsurface(); w1 = rpmw[3] - 0.5 * rsqr(mtot) / rad1; w2 = - (2*mpol + 3) / (3*mpol - npol + 5) * rsqr(mtot) / rad1; eprintf("[%s.polysolve: nstep = %3d W = %f error = %f]\n", getprog(), nstep, w2, (w1 - w2)/w2); gsl_odeiv2_driver_free(drv); h = 0.5 * h; // refine step-size by 2 } while (nstep < MAXSTEP/4); polyscale(); pmspline = gsl_spline_alloc(gsl_interp_cspline, nstep); gsl_spline_init(pmspline, rtab, ptab, nstep); if (listmodel) { printf("#%11s %11s %11s\n", "radius", "mass", "potential"); for (int i = 0; i < nstep - 1; i++) printf(" %11.6f %11.6f %11.6f\n", rtab[i], mtab[i], ptab[i]); printf(" %11.6f %11.6f %11.6f\n", rad1, mtot, phi1); } }
bool CBirrtProblem::DoGeneralIK(ostream& sout, istream& sinput) { //lock mutex EnvironmentMutex::scoped_lock lockenv(GetEnv()->GetMutex()); RAVELOG_DEBUG("Starting General IK...\n"); TransformMatrix tmtarg; TransformMatrix tmrobot; Transform Ttarg; Vector cogtarg; bool bExecute = false; bool bBalance = false; bool bGetTime = false; bool bReturnSolved = false; bool bNoRot = false; bool bReturnClosest = false; std::vector<string> supportlinks; std::vector<dReal> ikparams; std::vector<dReal> supportpolyx; std::vector<dReal> supportpolyy; dReal temp; string cmd; Vector polyscale(1.0,1.0,1.0); Vector polytrans(0,0,0); while(!sinput.eof()) { sinput >> cmd; if( !sinput ) break; if( stricmp(cmd.c_str(), "nummanips") == 0 ) { sinput >> temp; ikparams.push_back(temp); } else if( stricmp(cmd.c_str(), "exec") == 0 ) {