int Interval::complementary(Interval& c1, Interval& c2) const {
if (is_empty() || is_degenerated()) { // x.is_empty() should not happen if called from compl()
c1=Interval::ALL_REALS;
c2=Interval::EMPTY_SET;
return 1;
}
else {
if (lb()>NEG_INFINITY) {
c1=Interval(NEG_INFINITY,lb());
if (ub()<POS_INFINITY) {
c2=Interval(ub(),POS_INFINITY);
return 2;
} else {
c2=Interval::EMPTY_SET;
return 1;
}
} else if (ub()<POS_INFINITY) {
c1=Interval(ub(),POS_INFINITY);
c2=Interval::EMPTY_SET;
return 1;
} else {
c1=c2=Interval::EMPTY_SET;
return 0;
}
}
}
double TRAC_IK::manipPenalty(const KDL::JntArray& arr) {
double penalty = 1.0;
for (uint i=0; i< arr.data.size(); i++) {
if (types[i] == KDL::BasicJointType::Continuous)
continue;
double range = ub(i)-lb(i);
penalty *= ((arr(i)-lb(i))*(ub(i)-arr(i))/(range*range));
}
return std::max(0.0,1.0 - exp(-1*penalty));
}
void TRAC_IK::normalize_limits(const KDL::JntArray& seed, KDL::JntArray& solution) {
// Make sure rotational joint values are within 1 revolution of middle of
// limits; then ensure joint limits are met.
bool improved = false;
for (uint i=0; i<lb.data.size(); i++) {
if (types[i] == KDL::BasicJointType::TransJoint)
continue;
double target = seed(i);
if (types[i] == KDL::BasicJointType::RotJoint && types[i]!=KDL::BasicJointType::Continuous)
target = (ub(i)+lb(i))/2.0;
double val = solution(i);
if (val > target+M_PI) {
//Find actual angle offset
double diffangle = fmod(val-target,2*M_PI);
// Add that to upper bound and go back a full rotation
val = target + diffangle - 2*M_PI;
}
if (val < target-M_PI) {
//Find actual angle offset
double diffangle = fmod(target-val,2*M_PI);
// Add that to upper bound and go back a full rotation
val = target - diffangle + 2*M_PI;
}
if (types[i]==KDL::BasicJointType::Continuous) {
solution(i) = val;
continue;
}
if (val > ub(i)) {
//Find actual angle offset
double diffangle = fmod(val-ub(i),2*M_PI);
// Add that to upper bound and go back a full rotation
val = ub(i) + diffangle - 2*M_PI;
}
if (val < lb(i)) {
//Find actual angle offset
double diffangle = fmod(lb(i)-val,2*M_PI);
// Add that to upper bound and go back a full rotation
val = lb(i) - diffangle + 2*M_PI;
}
solution(i) = val;
}
}
Key sample_key(const std::unordered_map<Key, T>& map, Generator& rng) {
assert(!map.empty());
std::uniform_int_distribution<std::size_t> ub(0, map.bucket_count() - 1);
while (true) {
std::size_t bucket = ub(rng);
std::size_t bsize = map.bucket_size(bucket);
if (bsize > 0) {
std::uniform_int_distribution<std::size_t> ui(0, bsize - 1);
return std::next(map.begin(bucket), ui(rng))->first;
}
}
}
bool OsiRowCut::infeasible(const OsiSolverInterface &) const
{
if (lb() > ub())
return true;
return false;
}
/*--------------------------------------------------------*/
int NOMAD::TGP_Model::filter_and_sort_X
(const std::vector<const NOMAD::Eval_Point *> &X ,
const NOMAD::Point *center ,
std::list<const NOMAD::Eval_Point *> &filtered_X) const
{
NOMAD::Point alt_center;
const NOMAD::Eval_Point *cur = NULL;
int p0 = X.size() , i;
// alternate center if center==NULL:
if (!center)
{
int j;
NOMAD::Point lb(_n0) , ub(_n0);
for (i = 0 ; i < p0 ; ++i)
{
cur = X[i];
if (test_interpolation_point(cur))
{
for (j = 0 ; j < _n0 ; ++j)
{
if (!lb[j].is_defined() || (*cur)[j] < lb[j])
lb[j] = (*cur)[j];
if (!ub[j].is_defined() || (*cur)[j] > ub[j])
ub[j] = (*cur)[j];
}
}
}
alt_center = NOMAD::Point(_n0);
for (j = 0 ; j < _n0 ; ++j)
alt_center[j] = (lb[j] + ub[j]) / 2.0;
}
// X_tmp is used to sort the points:
std::multiset<NOMAD::Model_Sorted_Point> tmp_X;
for (i = 0 ; i < p0 ; ++i)
{
cur = X[i];
// test if the interpolation point is valid for interpolation:
if (test_interpolation_point(cur))
{
NOMAD::Model_Sorted_Point sorted_pt
(&NOMAD::Cache::get_modifiable_point(*cur) ,
(center) ? *center : alt_center);
tmp_X.insert(sorted_pt);
}
}
// copy the set X_tmp to filtered_X:
std::multiset<NOMAD::Model_Sorted_Point>::const_iterator it , end = tmp_X.end();
for (it = tmp_X.begin() ; it != end ; ++it)
filtered_X.push_back(static_cast<NOMAD::Eval_Point *>(it->get_point()));
return filtered_X.size();
}
void TRAC_IK::initialize() {
assert(chain.getNrOfJoints()==lb.data.size());
assert(chain.getNrOfJoints()==ub.data.size());
jacsolver.reset(new KDL::ChainJntToJacSolver(chain));
nl_solver.reset(new NLOPT_IK::NLOPT_IK(chain,lb,ub,maxtime,eps,NLOPT_IK::SumSq));
iksolver.reset(new KDL::ChainIkSolverPos_TL(chain,lb,ub,maxtime,eps,true,true));
for (uint i=0; i<chain.segments.size(); i++) {
std::string type = chain.segments[i].getJoint().getTypeName();
if (type.find("Rot")!=std::string::npos) {
if (ub(types.size())>=std::numeric_limits<float>::max() &&
lb(types.size())<=std::numeric_limits<float>::lowest())
types.push_back(KDL::BasicJointType::Continuous);
else
types.push_back(KDL::BasicJointType::RotJoint);
}
else if (type.find("Trans")!=std::string::npos)
types.push_back(KDL::BasicJointType::TransJoint);
}
assert(types.size()==lb.data.size());
threads.create_thread(boost::bind(&boost::asio::io_service::run,
&io_service));
threads.create_thread(boost::bind(&boost::asio::io_service::run,
&io_service));
initialized = true;
}
void ADMMCut::CalculateX()
{
// Construct Hessian Matrix for D-Qp problem
SpMat Q;
CalculateQ(D_, Q);
SpMat H2 = Q.transpose() * Q;
SpMat H = 2 * H1_ + penalty_ * H2;
// Construct Linear coefficient for x-Qp problem
a_ = Q.transpose() * lambda_;
// Inequality constraints A*x <= b
SpMat A(6 * Fd_, 6 * Fd_);
A.setZero();
VX b(6 * Fd_);
b.setZero();
// Variable constraints x >= lb, x <= ub
VX lb(Nd_), ub(Nd_);
lb.setZero();
ub.setOnes();
qp_->solve(H, a_, A, b, W_, d_, lb, ub, x_, NULL, NULL, debug_);
}
ModEvent
SetVarImp::excludeI_full(Space& home, int mi, int ma, I& iterator) {
Iter::Ranges::SingletonAppend<I> si(mi,ma,iterator);
if (lub.excludeI(home, si)) {
BndSetRanges ub(lub);
BndSetRanges lb(glb);
if (!Iter::Ranges::subset(lb,ub)) {
glb.become(home, lub);
glb.card(glb.size());
lub.card(glb.size());
return ME_SET_FAILED;
}
ModEvent me = ME_SET_LUB;
if (cardMax() > lub.size()) {
lub.card(lub.size());
if (cardMin() > cardMax()) {
glb.become(home, lub);
glb.card(glb.size());
lub.card(glb.size());
return ME_SET_FAILED;
}
me = ME_SET_CLUB;
}
if (cardMax() == lub.size() && cardMin() == cardMax()) {
glb.become(home, lub);
me = ME_SET_VAL;
}
SetDelta d;
return notify(home, me, d);
}
return ME_SET_NONE;
}
mga_target_event::mga_target_event(
const kep_toolbox::plantes::planet_ptr start,
const kep_toolbox::planet::planet_ptr end,
const kep_toolbox::epoch t_end,
double T_max,
bool discount_launcher
):base(6), m_start(start), m_end(end), m_t_end(t_end), m_T_max(T_max), m_discount_launcher(discount_launcher)
{
// We check that all planets have equal central body
if (start->get_mu_central_body() != end->get_mu_central_body()) {
pagmo_throw(value_error,"The planets do not all have the same mu_central_body");
}
// We check that T_max is positive
if (T_max <= 0) {
pagmo_throw(value_error,"T_max must be larger than zero");
}
// Now setting the problem bounds
decision_vector lb(6,0.0), ub(6,1.0);
lb[0] = 1.; lb[5] = 1e-5;
ub[0] = T_max; ub[2] = 12000.0; ub[5] = 1-1e-5;
set_bounds(lb,ub);
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs!=3 || nlhs != 8)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs","Usage [num_cnst,cnst_val,iAfun,jAvar,A,cnst_name,lb,ub] = testMultipleTimeLinearPostureConstraintmex(kinCnst,q,t)");
}
MultipleTimeLinearPostureConstraint* cnst = (MultipleTimeLinearPostureConstraint*) getDrakeMexPointer(prhs[0]);
int n_breaks = mxGetNumberOfElements(prhs[2]);
double* t_ptr = new double[n_breaks];
memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks);
int nq = cnst->getRobotPointer()->num_positions;
MatrixXd q(nq,n_breaks);
if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs","Argument 2 must be of size nq*n_breaks");
}
memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks);
int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks);
VectorXd c(num_cnst);
cnst->feval(t_ptr,n_breaks,q,c);
VectorXi iAfun;
VectorXi jAvar;
VectorXd A;
cnst->geval(t_ptr,n_breaks,iAfun,jAvar,A);
std::vector<std::string> cnst_names;
cnst->name(t_ptr,n_breaks,cnst_names);
VectorXd lb(num_cnst);
VectorXd ub(num_cnst);
cnst->bounds(t_ptr,n_breaks,lb,ub);
VectorXd iAfun_tmp(iAfun.size());
VectorXd jAvar_tmp(jAvar.size());
for(int i = 0;i<iAfun.size();i++)
{
iAfun_tmp(i) = (double) iAfun(i)+1;
jAvar_tmp(i) = (double) jAvar(i)+1;
}
plhs[0] = mxCreateDoubleScalar((double) num_cnst);
plhs[1] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[1]),c.data(),sizeof(double)*num_cnst);
plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[2]),iAfun_tmp.data(),sizeof(double)*iAfun_tmp.size());
plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[3]),jAvar_tmp.data(),sizeof(double)*jAvar_tmp.size());
plhs[4] = mxCreateDoubleMatrix(A.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[4]),A.data(),sizeof(double)*A.size());
int name_ndim = 1;
mwSize name_dims[] = {(mwSize) num_cnst};
plhs[5] = mxCreateCellArray(name_ndim,name_dims);
mxArray *name_ptr;
for(int i = 0;i<num_cnst;i++)
{
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[5],i,name_ptr);
}
plhs[6] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[6]),lb.data(),sizeof(double)*num_cnst);
memcpy(mxGetPrSafe(plhs[7]),ub.data(),sizeof(double)*num_cnst);
delete[] t_ptr;
}
void glpk_wrapper::set_domain(box const & b) {
assert(!b.is_empty());
changed = true;
domain = b;
for (unsigned int i = 0; i < b.size(); i++) {
auto interval = b[i];
double lb = interval.lb();
double ub = interval.ub();
if (lb == NEG_INFINITY) {
if (ub == POS_INFINITY) {
glp_set_col_bnds(lp, i+1, GLP_FR, lb, ub);
} else {
glp_set_col_bnds(lp, i+1, GLP_UP, lb, ub);
}
} else {
if (ub == POS_INFINITY) {
glp_set_col_bnds(lp, i+1, GLP_LO, lb, ub);
} else {
if (lb == ub) {
glp_set_col_bnds(lp, i+1, GLP_FX, lb, ub);
} else {
glp_set_col_bnds(lp, i+1, GLP_DB, lb, ub);
}
}
}
}
}
bool raycast(game* g, vec2 from, vec2 to) {
ivec2 cur(fast_floor(from.x), fast_floor(from.y));
ivec2 end(fast_floor(to.x), fast_floor(to.y));
ivec2 sign(sign(to.x - from.x), sign(to.y - from.y));
vec2 abs_delta(abs(to.x - from.x), abs(to.y - from.y));
vec2 delta_t(vec2(1.0f) / abs_delta);
vec2 lb(to_vec2(cur));
vec2 ub(lb + vec2(1.0f));
vec2 t(vec2((from.x > to.x) ? (from.x - lb.x) : (ub.x - from.x), (from.y > to.y) ? (from.y - lb.y) : (ub.y - from.y)) / abs_delta);
for(;;) {
if (g->is_raycast_solid(cur.x, cur.y))
return true;
if (t.x <= t.y) {
if (cur.x == end.x) break;
t.x += delta_t.x;
cur.x += sign.x;
}
else {
if (cur.y == end.y) break;
t.y += delta_t.y;
cur.y += sign.y;
}
}
return false;
}
double Interval::delta(const Interval& x) const {
if (is_empty()) return 0;
if (x.is_empty()) return diam();
// ** warning **
// checking if *this or x is infinite by
// testing if the lower/upper bounds are -oo/+oo
// is not enough because diam() may return +oo even
// with finite bounds (e.g, very large intervals like [-DBL_MAX,DBL_MAX]).
// ("almost-unboundedness")
volatile double d=diam();
volatile double dx=x.diam();
// furthermore, if these variables are not declared volatile
// conditions like d==POS_INFINITY are evaluated
// to FALSE for intervals like [-DBL_MAX,DBL_MAX] (with -O3 option)
// while the returned expression (d-dx) evaluates to +oo (instead of 0).
if (d==POS_INFINITY) {
//cout << "d=" << d << " dx=" << dx << endl;
if (dx==POS_INFINITY) {
double left=(x.lb()==NEG_INFINITY? 0 : x.lb()-lb());
double right=(x.ub()==POS_INFINITY? 0 : ub()-x.ub());
//cout << "left=" << left << " right=" << right << endl;
return left+right;
} else
return POS_INFINITY;
}
else return d-dx;
}
int TwoNodeLink::update()
{
int errCode = 0;
// get global trial response
const Vector &dsp1 = theNodes[0]->getTrialDisp();
const Vector &dsp2 = theNodes[1]->getTrialDisp();
const Vector &vel1 = theNodes[0]->getTrialVel();
const Vector &vel2 = theNodes[1]->getTrialVel();
int numDOF2 = numDOF/2;
Vector ug(numDOF), ugdot(numDOF), uldot(numDOF);
for (int i=0; i<numDOF2; i++) {
ug(i) = dsp1(i); ugdot(i) = vel1(i);
ug(i+numDOF2) = dsp2(i); ugdot(i+numDOF2) = vel2(i);
}
// transform response from the global to the local system
ul.addMatrixVector(0.0, Tgl, ug, 1.0);
uldot.addMatrixVector(0.0, Tgl, ugdot, 1.0);
// transform response from the local to the basic system
ub.addMatrixVector(0.0, Tlb, ul, 1.0);
ubdot.addMatrixVector(0.0, Tlb, uldot, 1.0);
//ub = (Tlb*Tgl)*ug;
//ubdot = (Tlb*Tgl)*ugdot;
// set trial response for material models
for (int i=0; i<numDir; i++)
errCode += theMaterials[i]->setTrialStrain(ub(i),ubdot(i));
return errCode;
}
TEST(BoundingBox, Intersection){
Point lb(1,2,0);
Point ub(2,4,0);
BoundingBox bb1(lb, ub);
BoundingBox bb2(lb, ub);
// Intersection should return an bounding box equal to bb2 and bb1,
// they are the same bb
BoundingBox bbIntersection = bb2.intersection(bb1);
EXPECT_TRUE( bbIntersection.getLb().getX() == bb2.getLb().getX() &&
bbIntersection.getLb().getY() == bb2.getLb().getY() &&
bbIntersection.getLb().getZ() == bb2.getLb().getZ());
EXPECT_TRUE( bbIntersection.getUb().getX() == bb2.getUb().getX() &&
bbIntersection.getUb().getY() == bb2.getUb().getY() &&
bbIntersection.getUb().getZ() == bb2.getUb().getZ());
Point lb3(2,3,0);
Point ub3(2,4,0);
BoundingBox bb3(lb3, ub3);
bbIntersection = bb2.intersection(bb3);
// lower bound is the value of of bb3
EXPECT_TRUE( bbIntersection.getLb().getX() == lb3.getX() &&
bbIntersection.getLb().getY() == lb3.getY() &&
bbIntersection.getLb().getZ() == lb3.getZ());
// upper bound should not change
EXPECT_TRUE( bbIntersection.getUb().getX() == bb2.getUb().getX() &&
bbIntersection.getUb().getY() == bb2.getUb().getY() &&
bbIntersection.getUb().getZ() == bb2.getUb().getZ());
}
void mexFunction(int nlhs,mxArray* plhs[], int nrhs, const mxArray * prhs[])
{
if(nrhs!=3 || nlhs != 7)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeKinCnstmex:BadInputs","Usage [type,num_cnst,cnst_val,dcnst_val,cnst_name,lb,ub] = testMultipleTimeKinCnstmex(kinCnst,q,t)");
}
MultipleTimeKinematicConstraint* cnst = (MultipleTimeKinematicConstraint*) getDrakeMexPointer(prhs[0]);
int n_breaks = mxGetNumberOfElements(prhs[2]);
double* t_ptr = new double[n_breaks];
memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks);
int nq = cnst->getRobotPointer()->num_positions;
MatrixXd q(nq,n_breaks);
if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeKinCnstmex:BadInputs","Argument 2 must be of size nq*n_breaks");
}
memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks);
int type = cnst->getType();
int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks);
VectorXd c(num_cnst);
MatrixXd dc(num_cnst,nq*n_breaks);
cnst->eval(t_ptr,n_breaks,q,c,dc);
VectorXd lb(num_cnst);
VectorXd ub(num_cnst);
cnst->bounds(t_ptr,n_breaks,lb,ub);
std::vector<std::string> cnst_names;
cnst->name(t_ptr,n_breaks,cnst_names);
int retvec_size;
if(num_cnst == 0)
{
retvec_size = 0;
}
else
{
retvec_size = 1;
}
plhs[0] = mxCreateDoubleScalar((double) type);
plhs[1] = mxCreateDoubleScalar((double) num_cnst);
plhs[2] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL);
memcpy(mxGetPrSafe(plhs[2]),c.data(),sizeof(double)*num_cnst);
plhs[3] = mxCreateDoubleMatrix(num_cnst,nq*n_breaks,mxREAL);
memcpy(mxGetPrSafe(plhs[3]),dc.data(),sizeof(double)*num_cnst*nq*n_breaks);
int name_ndim = 1;
mwSize name_dims[] = {(mwSize) num_cnst};
plhs[4] = mxCreateCellArray(name_ndim,name_dims);
mxArray *name_ptr;
for(int i = 0;i<num_cnst;i++)
{
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[4],i,name_ptr);
}
plhs[5] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL);
plhs[6] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL);
memcpy(mxGetPrSafe(plhs[5]),lb.data(),sizeof(double)*num_cnst);
memcpy(mxGetPrSafe(plhs[6]),ub.data(),sizeof(double)*num_cnst);
delete[] t_ptr;
}
std::basic_ostream<Char,Traits>&
operator <<(std::basic_ostream<Char,Traits>& os, const ConstSetView& x) {
std::basic_ostringstream<Char,Traits> s;
s.copyfmt(os); s.width(0);
LubRanges<ConstSetView> ub(x);
printBound(s, ub);
s << "#(" << x.cardMin() << ")";
return os << s.str();
}
std::basic_ostream<Char,Traits>&
operator <<(std::basic_ostream<Char,Traits>& os, const SetView& x) {
std::basic_ostringstream<Char,Traits> s;
s.copyfmt(os); s.width(0);
LubRanges<SetView> ub(x);
GlbRanges<SetView> lb(x);
print(s, x.assigned(), lb, ub, x.cardMin(), x.cardMax()) ;
return os << s.str();
}
forceinline int
ComplementView<View>::glbMin(void) const {
LubRanges<View> ub(x);
RangesCompl<LubRanges<View> > ubc(ub);
if (ubc()) {
return ubc.min();
} else {
return BndSet::MIN_OF_EMPTY;
}
}
//----------------------------------------------------------------
// == operator
//-------------------------------------------------------------------
bool
OsiRowCut::operator==(const OsiRowCut& rhs) const
{
if ( this->OsiCut::operator!=(rhs) ) return false;
if ( row() != rhs.row() ) return false;
if ( lb() != rhs.lb() ) return false;
if ( ub() != rhs.ub() ) return false;
return true;
}
/// is this expression integer?
virtual inline bool isInteger () {
if (isDefinedInteger ())
return true;
register CouNumber l = lb ();
return ((l == ub ()) && (COUENNE_round (l) == l));
//CouNumber l = (*(Lb ())) ();
//return (::isInteger (l) && (fabs (l - (*(Ub ())) ()) < COUENNE_EPS));
}
forceinline int
ComplementView<View>::glbMax(void) const {
LubRanges<View> ub(x);
RangesCompl<LubRanges<View> > ubc(ub);
if (ubc()) {
while (ubc()) ++ubc;
return ubc.max();
} else {
return BndSet::MAX_OF_EMPTY;
}
}
void ObjectInspector::ObjectInspectorPrivate::setFormWindow(QDesignerFormWindowInterface *fwi)
{
const bool blocked = m_treeView->selectionModel()->blockSignals(true);
{
UpdateBlocker ub(m_treeView);
setFormWindowBlocked(fwi);
}
m_treeView->update();
m_treeView->selectionModel()->blockSignals(blocked);
}
/**
* Constructs a global optimization problem (box-bounded, continuous) representing an interplanetary trajectory modelled
* as a Multiple Gravity Assist trajectory that allows one only Deep Space Manouvre between each leg.
*
* @param[in] seq std::vector of kep_toolbox::planet_ptr containing the encounter sequence for the trajectoty (including the initial planet)
* @param[in] t0_l kep_toolbox::epoch representing the lower bound for the launch epoch
* @param[in] t0_u kep_toolbox::epoch representing the upper bound for the launch epoch
* @param[in] tof time-of-flight vector containing lower and upper bounds (in days) for the various legs time of flights
* @param[in] Tmax maximum time of flight
* @param[in] Dmin minimum distance from Jupiter (for radiation protection)
*
* @throws value_error if the planets in seq do not all have the same central body gravitational constant
* @throws value_error if tof has a size different from seq.size()
*/
mga_incipit_cstrs::mga_incipit_cstrs(
const std::vector<kep_toolbox::planets::planet_ptr> seq,
const kep_toolbox::epoch t0_l,
const kep_toolbox::epoch t0_u,
const std::vector<std::vector<double> > tof,
double Tmax,
double Dmin) : base(4*seq.size(),0,1,2,2,1E-3), m_tof(tof), m_tmax(Tmax), m_dmin(Dmin)
{
// We check that all planets have equal central body
std::vector<double> mus(seq.size());
for (std::vector<kep_toolbox::planets::planet_ptr>::size_type i = 0; i< seq.size(); ++i) {
mus[i] = seq[i]->get_mu_central_body();
}
if ((unsigned int)std::count(mus.begin(), mus.end(), mus[0]) != mus.size()) {
pagmo_throw(value_error,"The planets do not all have the same mu_central_body");
}
// We check the consistency of the time of flights
if (tof.size() != seq.size()) {
pagmo_throw(value_error,"The time-of-flight vector (tof) has the wrong length");
}
for (size_t i = 0; i < tof.size(); ++i) {
if (tof[i].size()!=2) pagmo_throw(value_error,"Each element of the time-of-flight vector (tof) needs to have dimension 2 (lower and upper bound)");
}
// Filling in the planetary sequence data member. This requires to construct the polymorphic planets via their clone method
for (std::vector<kep_toolbox::planets::planet_ptr>::size_type i = 0; i < seq.size(); ++i) {
m_seq.push_back(seq[i]->clone());
}
// Now setting the problem bounds
size_type dim(4*m_tof.size());
decision_vector lb(dim), ub(dim);
// First leg
lb[0] = t0_l.mjd2000(); ub[0] = t0_u.mjd2000();
lb[1] = 0; lb[2] = 0; ub[1] = 1; ub[2] = 1;
lb[3] = m_tof[0][0]; ub[3] = m_tof[0][1];
// Successive legs
for (std::vector<kep_toolbox::planets::planet_ptr>::size_type i = 1; i < m_tof.size(); ++i) {
lb[4*i] = - 2 * boost::math::constants::pi<double>(); ub[4*i] = 2 * boost::math::constants::pi<double>();
lb[4*i+1] = 1.1; ub[4*i+1] = 30;
lb[4*i+2] = 1e-5; ub[4*i+2] = 1-1e-5;
lb[4*i+3] = m_tof[i][0]; ub[4*i+3] = m_tof[i][1];
}
// Adjusting the minimum and maximum allowed fly-by rp to the one defined in the kep_toolbox::planet class
for (std::vector<kep_toolbox::planets::planet_ptr>::size_type i = 0; i < m_tof.size()-1; ++i) {
lb[4*i+5] = m_seq[i]->get_safe_radius() / m_seq[i]->get_radius();
ub[4*i+5] = (m_seq[i]->get_radius() + 2000000) / m_seq[i]->get_radius(); //from gtoc6 problem description
}
set_bounds(lb,ub);
}
unsigned int
One::runUnweighted()
{
trace_msg( weakconstraints, 1, "Starting algorithm ONE" );
computeAssumptions();
initInUnsatCore();
solver.setComputeUnsatCores( true );
solver.turnOffSimplifications();
while( solver.solve( assumptions ) == INCOHERENT )
{
if( !foundUnsat() )
return INCOHERENT;
assumptions.clear();
computeAssumptions();
}
assert_msg( lb() == ub(), lb() << " != " << ub() );
return OPTIMUM_FOUND;
}
/**
* Description not yet available.
* \param
*/
dmatrix_position::dmatrix_position(int min,int max)
: lb(min,max), ub(min,max), ptr(min,max)
{
row_min=min;
row_max=max;
for (int i=row_min;i<=row_max;i++)
{
lb(i)=0;
ub(i)=-1;
ptr(i)=0;
}
}
vector<vector<int> > fourSum(vector<int> &num, int target) {
int n=num.size();
s.clear();
ans.clear();
node t;
for(int i=0;i<n;i++)
for(int j=i+1;j<n;j++)
{
t.val=num[i]+num[j];
t.i1=i;
t.i2=j;
s.push_back(t);
}
sort(s.begin(),s.end());
for (int i = 0; i < s.size(); i++)
{
int begin, end, mid;
int val = target - s[i].val;
begin = lb(i+1, s.size() -1, val);
end = ub(i+1, s.size() -1, val);
for (int j = begin; j <=end; j++)
{
if (s[j].i1 == s[i].i1) continue;
if (s[j].i2 == s[i].i1) continue;
if (s[j].i1 == s[i].i2) continue;
if (s[j].i2 == s[i].i2) continue;
vector<int> tmp;
tmp.push_back( num[ s[i].i1] );
tmp.push_back( num[ s[i].i2] );
tmp.push_back( num[ s[j].i1] );
tmp.push_back( num[ s[j].i2] );
sort(tmp.begin(), tmp.end());
if ( find(ans.begin(), ans.end(), tmp) == ans.end() )
ans.push_back( tmp );
}
}
return ans;
}
/**
* Description not yet available.
* \param
*/
dmatrix_position::dmatrix_position(const dmatrix& m)
: lb(m.rowmin(),m.rowmax()), ub(m.rowmin(),m.rowmax()),
ptr(m.rowmin(),m.rowmax())
{
row_min=m.rowmin();
row_max=m.rowmax();
for (int i=row_min;i<=row_max;i++)
{
lb(i)=m(i).indexmin();
ub(i)=m(i).indexmax();
ptr(i)=m(i).get_v();
}
}
unsigned int
One::run()
{
if( disjCoresPreprocessing && !disjointCorePreprocessing() )
return INCOHERENT;
if( ub() == lb() )
return OPTIMUM_FOUND;
if( wasp::Options::stratification && solver.isWeighted( level() ) )
return runWeighted();
return runUnweighted();
}