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 ) {
