void
MultiGrid::solve (MultiFab& _sol,
const MultiFab& _rhs,
Real _eps_rel,
Real _eps_abs,
LinOp::BC_Mode bc_mode)
{
//
// Prepare memory for new level, and solve the general boundary
// value problem to within relative error _eps_rel. Customized
// to solve at level=0.
//
const int level = 0;
prepareForLevel(level);
//
// Copy the initial guess, which may contain inhomogeneous boundray conditions,
// into both "initialsolution" (to be added back later) and into "cor[0]" which
// we will only use here to compute the residual, then will set back to 0 below
//
initialsolution->copy(_sol);
cor[level]->copy(_sol);
//
// Put the problem in residual-correction form: we will now use "rhs[level
// the initial residual (rhs[0]) rather than the initial RHS (_rhs)
// to begin the solve.
//
Lp.residual(*rhs[level],_rhs,*cor[level],level,bc_mode);
//
// Now initialize correction to zero at this level (auto-filled at levels below)
//
(*cor[level]).setVal(0.0); //
//
// Elide a reduction by doing these together.
//
Real tmp[2] = { norm_inf(_rhs,true), norm_inf(*rhs[level],true) };
ParallelDescriptor::ReduceRealMax(tmp,2);
if ( ParallelDescriptor::IOProcessor() && verbose > 0)
{
Spacer(std::cout, level);
std::cout << "MultiGrid: Initial rhs = " << tmp[0] << '\n';
std::cout << "MultiGrid: Initial residual = " << tmp[1] << '\n';
}
if (tmp[1] == 0.0)
return;
//
// We can now use homogeneous bc's because we have put the problem into residual-correction form.
//
if ( !solve_(_sol, _eps_rel, _eps_abs, LinOp::Homogeneous_BC, tmp[0], tmp[1]) )
BoxLib::Error("MultiGrid:: failed to converge!");
}
int SuGame::solve(std::vector<SuFieldCell> &field)
{
std::vector<SuFieldCell> fieldCopy = field;
if(checkOnConflict(field))
return NOT_SOLVED;
int status = solve_(fieldCopy);
if(status == SOLVED)
field = fieldCopy;
return status;
}
void
MCMultiGrid::solve (MultiFab& _sol,
const MultiFab& _rhs,
Real _eps_rel,
Real _eps_abs,
MCBC_Mode bc_mode)
{
BL_ASSERT(numcomps == _sol.nComp());
//
// Prepare memory for new level, and solve the general boundary
// value problem to within relative error _eps_rel. Customized
// to solve at level=0
//
int level = 0;
prepareForLevel(level);
residualCorrectionForm(*rhs[level],_rhs,*cor[level],_sol,bc_mode,level);
if (!solve_(_sol, _eps_rel, _eps_abs, MCHomogeneous_BC, level))
BoxLib::Error("MCMultiGrid::solve(): failed to converge!");
}
void solve(double *mat, double *b, double *v, int n)
{
int i,j;
double *mat2 = (double *)malloc(n*n*sizeof(double));
/* lapack is going to put the lu-decomp into the matrix,
* so make a copy and convert to column-major order */
for(i=0;i<n;i++) {
for(j=0;j<n;j++) {
mat2[n*i+j] = mat[n*j+i];
}
}
/* copy b into v */
for(i=0;i<n;i++) {
v[i] = b[i];
}
solve_(mat2, v, &n);
free(mat2);
return;
}
/*< >*/
/* Subroutine */ int iapibrdf_(integer *pild, doublereal *pxlt, doublereal *
prl, doublereal *ptl, doublereal *prs, integer *pihs, doublereal *pc,
integer *mu, integer *np, doublereal *rm, doublereal *rp, doublereal *
brdfint)
{
/* System generated locals */
integer rm_offset, brdfint_dim1, brdfint_offset, i__1, i__2;
/* Builtin functions */
integer s_wsle(cilist *), do_lio(integer *, integer *, char *, ftnlen),
e_wsle();
/* Subroutine */ int s_stop(char *, ftnlen);
double acos(doublereal);
/* Local variables */
integer j, k;
doublereal fi, mu1, mu2;
extern /* Subroutine */ int lad_();
integer ihs;
extern doublereal ro_1__(doublereal *, doublereal *, doublereal *,
doublereal *);
doublereal phi_i__, phi_v__;
extern /* Subroutine */ int solve_(doublereal *), gauleg_(doublereal *,
doublereal *, doublereal *, doublereal *, integer *);
doublereal theta_i__, theta_v__;
/* Fortran I/O blocks */
static cilist io___2 = { 0, 6, 0, 0, 0 };
static cilist io___3 = { 0, 6, 0, 0, 0 };
static cilist io___4 = { 0, 6, 0, 0, 0 };
static cilist io___5 = { 0, 6, 0, 0, 0 };
static cilist io___6 = { 0, 6, 0, 0, 0 };
static cilist io___7 = { 0, 6, 0, 0, 0 };
static cilist io___8 = { 0, 6, 0, 0, 0 };
static cilist io___9 = { 0, 6, 0, 0, 0 };
static cilist io___10 = { 0, 6, 0, 0, 0 };
static cilist io___11 = { 0, 6, 0, 0, 0 };
static cilist io___12 = { 0, 6, 0, 0, 0 };
static cilist io___13 = { 0, 6, 0, 0, 0 };
static cilist io___14 = { 0, 6, 0, 0, 0 };
/* interface between the computer code of the model of Iaquinta and Pinty
*/
/* the computer code is courtesy of Jean Ianquinta */
/* see module IAPITOOLS.f for a complete description */
/*< integer np,mu >*/
/*< real rm(-mu:mu),rp(np),brdfint(-mu:mu,np) >*/
/*< real ro_1 >*/
/*< integer iwr,k,j >*/
/*< integer pild,pihs >*/
/*< real pxlt,prl,ptl,prs,pc >*/
/*< logical ier >*/
/*< common/sixs_ier/iwr,ier >*/
/*< real mu1,mu2,fi >*/
/*< real pi >*/
/* begin of Iaquinta and Pinty model parameter and declaration */
/*< parameter (Pi=3.141592653589793) >*/
/*< common /gauss_m/xgm (20),wgm (20),n >*/
/*< real xgm,wgm >*/
/*< integer n >*/
/*< common /p/xLt,Rl,Tl,Rs,c,ild >*/
/*< real xLt,Rl,Tl,Rs,c >*/
/*< integer ild >*/
/*< common /ld/a_ld,b_ld,c_ld,d_ld >*/
/*< real a_ld,b_ld,c_ld,d_ld >*/
/*< common /Ro/Ro_1_c,Ro_1_s,Ro_mult >*/
/*< real Ro_1_c,Ro_1_s,Ro_mult >*/
/*< real Theta_i,Phi_i >*/
/*< real Theta_v,Phi_v >*/
/*< integer ihs >*/
/* xLt = leaf area index */
/* Rl = leaf reflection coefficient (Bi-Lambertian) */
/* Tl = leaf transmission coefficient (Bi-Lambertian) */
/* ild = leaf angle distribution : */
/* 1 = planophile */
/* 2 = erectophile */
/* 3 = plagiophile */
/* 4 = extremophile */
/* 5 = uniform */
/* Rs = soil albedo */
/* c = 2*r*Lambda */
/* Ro_1_c = single scattering by the canopy term */
/* Ro_1_s = uncollided by the leaves (or singly scattered by */
/* the soil) radiation */
/* (Ro_1 = Ro_1_c + Ro_1_s) */
/* Ro_mult = multiple scattering */
/* transfer paramater to common / / parameter struture */
/*< ild=pild >*/
/* Parameter adjustments */
rm_offset = -(*mu);
rm -= rm_offset;
brdfint_dim1 = *mu - (-(*mu)) + 1;
brdfint_offset = -(*mu) + brdfint_dim1;
brdfint -= brdfint_offset;
--rp;
/* Function Body */
p_1.ild = *pild;
/*< Xlt=pXlt >*/
p_1.xlt = *pxlt;
/*< Rl=pRl >*/
p_1.rl = *prl;
/*< Tl=pTl >*/
p_1.tl = *ptl;
/*< Rs=pRs >*/
p_1.rs = *prs;
/*< ihs=pihs >*/
ihs = *pihs;
/*< c=pc >*/
p_1.c__ = *pc;
/* Check parameter validity */
/*< >*/
if (p_1.ild != 1 && p_1.ild != 2 && p_1.ild != 3 && p_1.ild != 4 &&
p_1.ild != 5) {
/*< print*,'Leaf angle distribution !' >*/
s_wsle(&io___2);
do_lio(&c__9, &c__1, "Leaf angle distribution !", 25L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (xlt.le.0.) then >*/
if (p_1.xlt <= 0.) {
/*< print*,'Leaf area index < 0. !' >*/
s_wsle(&io___3);
do_lio(&c__9, &c__1, "Leaf area index < 0. !", 22L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (xlt.lt.1.) then >*/
if (p_1.xlt < 1.) {
/*< print*,'Leaf area index < 1. !' >*/
s_wsle(&io___4);
do_lio(&c__9, &c__1, "Leaf area index < 1. !", 22L);
e_wsle();
/*< endif >*/
}
/*< if (xlt.gt.15.) then >*/
if (p_1.xlt > 15.) {
/*< print*,'Leaf area index > 15. !' >*/
s_wsle(&io___5);
do_lio(&c__9, &c__1, "Leaf area index > 15. !", 23L);
e_wsle();
/*< endif >*/
}
/*< if (Rl.lt.0.) then >*/
if (p_1.rl < 0.) {
/*< print*,'Leaf reflectance < 0. !' >*/
s_wsle(&io___6);
do_lio(&c__9, &c__1, "Leaf reflectance < 0. !", 23L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (Rl.gt..99) then >*/
if (p_1.rl > .99) {
/*< print*,'Leaf reflectance > .99 !' >*/
s_wsle(&io___7);
do_lio(&c__9, &c__1, "Leaf reflectance > .99 !", 24L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (Tl.lt.0.) then >*/
if (p_1.tl < 0.) {
/*< print*,'Leaf transmittance < 0. !' >*/
s_wsle(&io___8);
do_lio(&c__9, &c__1, "Leaf transmittance < 0. !", 25L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (Tl.gt..99) then >*/
if (p_1.tl > .99) {
/*< print*,'Leaf transmittance > .99 !' >*/
s_wsle(&io___9);
do_lio(&c__9, &c__1, "Leaf transmittance > .99 !", 26L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (Rl+Tl.gt..99) then >*/
if (p_1.rl + p_1.tl > .99) {
/*< print*,'Single scattering albedo > .99 !' >*/
s_wsle(&io___10);
do_lio(&c__9, &c__1, "Single scattering albedo > .99 !", 32L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (Rs.lt.0.) then >*/
if (p_1.rs < 0.) {
/*< print*,'Soil albedo < 0. !' >*/
s_wsle(&io___11);
do_lio(&c__9, &c__1, "Soil albedo < 0. !", 18L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (Rs.gt..99) then >*/
if (p_1.rs > .99) {
/*< print*,'Soil albedo > .99 !' >*/
s_wsle(&io___12);
do_lio(&c__9, &c__1, "Soil albedo > .99 !", 19L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (c.lt.0.) then >*/
if (p_1.c__ < 0.) {
/*< print*,'Hot-spot parameter < 0. !' >*/
s_wsle(&io___13);
do_lio(&c__9, &c__1, "Hot-spot parameter < 0. !", 25L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/*< if (c.gt.2.) then >*/
if (p_1.c__ > 2.) {
/*< print*,'Hot-spot parameter > 2. !' >*/
s_wsle(&io___14);
do_lio(&c__9, &c__1, "Hot-spot parameter > 2. !", 25L);
e_wsle();
/*< stop >*/
s_stop("", 0L);
/*< endif >*/
}
/* compute leaf area angle distribution */
/*< call lad >*/
lad_();
/* - Hot-spot parameter */
/*< if (ihs.eq.0) c=0. >*/
if (ihs == 0) {
p_1.c__ = 0.;
}
/*< mu1=rm(0) >*/
mu1 = rm[0];
/*< Theta_i=acos(mu1) >*/
theta_i__ = acos(mu1);
/*< Theta_i=Pi-Theta_i >*/
theta_i__ = 3.141592653589793 - theta_i__;
/* - Gauss's quadrature (n points) */
/*< n=10 >*/
gauss_m__1.n = 10;
/*< call gauleg (-1.,1.,xgm,wgm,n) >*/
gauleg_(&c_b52, &c_b53, gauss_m__1.xgm, gauss_m__1.wgm, &gauss_m__1.n);
/* - Computation of the multiple scattering (Ro_mult) */
/*< call solve (Theta_i) >*/
solve_(&theta_i__);
/*< do 1 k=1,np >*/
i__1 = *np;
for (k = 1; k <= i__1; ++k) {
/*< do 2 j=1,mu >*/
i__2 = *mu;
for (j = 1; j <= i__2; ++j) {
/*< mu2=rm(j) >*/
mu2 = rm[j];
/*< if (j.eq.mu) then >*/
if (j == *mu) {
/*< fi=rm(-mu) >*/
fi = rm[-(*mu)];
/*< else >*/
} else {
/*< fi=rp(k)+rm(-mu) >*/
fi = rp[k] + rm[-(*mu)];
/*< endif >*/
}
/*< Theta_v=acos(mu2) >*/
theta_v__ = acos(mu2);
/*< if (fi.lt.0.) fi=fi+2.*pi >*/
if (fi < 0.) {
fi += 6.2831853071795862;
}
/*< if (fi.gt.(2.*pi)) fi=fi-2.*pi >*/
if (fi > 6.2831853071795862) {
fi += -6.2831853071795862;
}
/*< Phi_i=fi >*/
phi_i__ = fi;
/*< Phi_v=0. >*/
phi_v__ = 0.;
/*< brdfint(j,k)=Ro_1(Theta_i,Phi_i,Theta_v,Phi_v)+Ro_mult >*/
brdfint[j + k * brdfint_dim1] = ro_1__(&theta_i__, &phi_i__, &
theta_v__, &phi_v__) + ro_1.ro_mult__;
/*< 2 continue >*/
/* L2: */
}
/*< 1 continue >*/
/* L1: */
}
/*< return >*/
return 0;
/*< end >*/
} /* iapibrdf_ */
/** Solves the motion planning problem */
std::vector<std::vector<double> > PathPlanner::solve(const std::vector<double> &start_state_vec, double timeout) {
std::vector<double> ss_vec;
ompl::base::ScopedState<> start_state(space_);
for (size_t i =0; i < dim_; i++) {
ss_vec.push_back(start_state_vec[i]);
start_state[i] = ss_vec[i];
}
std::vector<std::vector<double> > solution_vector;
boost::shared_ptr<MotionValidator> mv = boost::static_pointer_cast<MotionValidator>(si_->getMotionValidator());
/** Add the start state to the problem definition */
if (verbose_) {
cout << "Adding start state: ";
for (size_t i = 0; i < dim_; i++) {
cout << start_state[i] << ", ";
}
cout << endl;
}
/**if (!isValid(start_state.get())) {
cout << "Path planner: ERROR: Start state not valid!" << endl;
return solution_vector;
}*/
problem_definition_->addStartState(start_state);
ompl::base::GoalPtr gp(new ManipulatorGoalRegion(si_, robot_, goal_states_, ee_goal_position_, ee_goal_threshold_, false));
boost::dynamic_pointer_cast<ompl::base::GoalRegion>(gp)->setThreshold(ee_goal_threshold_);
problem_definition_->setGoal(gp);
if (check_linear_path_) {
bool collides = false;
std::vector<double> goal_state_vec = boost::dynamic_pointer_cast<ManipulatorGoalRegion>(gp)->sampleGoalVec();
std::vector<std::vector<double> > linear_path(genLinearPath(ss_vec, goal_state_vec));
for (size_t i = 1; i < linear_path.size(); i++) {
if (!(mv->checkMotion(linear_path[i - 1], linear_path[i], continuous_collision_))) {
collides = true;
break;
}
}
if (!collides) {
clear();
if (verbose_) {
cout << "Linear path is a valid solution. Returning linear path of length " << linear_path.size() << endl;
}
return augmentPath_(linear_path);
}
}
/** Solve the planning problem with a maximum of *timeout* seconds per attempt */
bool solved = false;
boost::timer t;
bool approximate_solution = true;
/**while (!solved || approximate_solution) {
solved = planner_->solve(4.0);
//solved = planner_->solve(10.0);
if (t.elapsed() > timeout) {
cout << "Timeout reached." << endl;
return solution_vector;
}
for (auto &solution : problem_definition_->getSolutions()) {
cout << "diff " << solution.difference_ << endl;
approximate_solution = false;
}
}**/
solved = solve_(timeout);
if (!solved) {
return solution_vector;
}
boost::shared_ptr<ompl::geometric::PathGeometric> solution_path = boost::static_pointer_cast<ompl::geometric::PathGeometric>(problem_definition_->getSolutionPath());
solution_vector.push_back(ss_vec);
/** We found a solution, so get the solution path */
if (verbose_) {
cout << "Solution found in " << t.elapsed() << " seconds." << endl;
cout << "Solution path has length " << solution_path->getStates().size() << endl;
}
std::vector<double> vals;
const bool cont_check = true;
for (size_t i=1; i<solution_path->getStates().size(); i++) {
vals.clear();
for (unsigned int j = 0; j < dim_; j++) {
//vals.push_back(solution_path->getState(i)->as<ompl::base::RealVectorStateSpace::StateType>()->values[j]);
vals.push_back(solution_path->getState(i)->as<shared::RealVectorStateSpace::StateType>()->values[j]);
}
solution_vector.push_back(vals);
}
clear();
return augmentPath_(solution_vector);
}
int SuGame::solve_(std::vector<SuFieldCell> &field)
{
int minRow = -1;
int minCol = -1;
QSet<int> minValues;
while(true)
{
minRow = -1;
for(int y = 0; y < 9; y++)
{
for(int x = 0; x < 9; x++)
{
int index = y * 9 + x;
if(field.at(index).value != 0)
continue;
QSet<int> possibleValues = findPossibleValues_(field, x, y);
int possibleValuesCount = possibleValues.size();
if(possibleValuesCount == 0)
return NOT_SOLVED;
if(possibleValuesCount == 1)
field[index].setNumber(*possibleValues.begin());
if(minRow < 0 || possibleValuesCount < minValues.size())
{
minRow = y;
minCol = x;
minValues = possibleValues;
}
}
}
if(minRow == -1)
{
return SOLVED;
}
else if(minValues.size() > 1)
{
break;
}
}
for(QSet<int>::iterator v = minValues.begin(); v != minValues.end(); ++v)
{
std::vector<SuFieldCell> fieldCopy = field;
fieldCopy[minRow * LEN_SIDE_OF_FIELD + minCol].setNumber(*v);
if(solve_(fieldCopy) == SOLVED)
{
field = fieldCopy;
return SOLVED;
}
}
return NOT_SOLVED;
}
bool solve( const vector< Coef >& b, vector< Coef >& x )
{
return solve_( b, x );
}