C++ box类代码示例

/**
 * Dividing the box in all n dimensions producing 2^n boxes of the same size
 * according to the precision vector e
 */
vector<box> box_factory::bisect(box b, map<string, pdrh::node*> e)
{
    std::map<std::string, std::vector<capd::interval>> tmp_m;
    std::map<std::string, capd::interval> m = b.get_map();
    //cout << "BISECTING ";
    for(auto it = m.cbegin(); it != m.cend(); it++)
    {
        //cout << it->first << ":" << it->second;
        if(capd::intervals::width(it->second) > pdrh::node_to_interval(e[it->first]).leftBound())
        {
            //cout << " yes" << endl;
            std::vector<capd::interval> tmp_v;
            tmp_v.push_back(capd::interval((it->second).leftBound(), (it->second).mid().rightBound()));
            tmp_v.push_back(capd::interval((it->second).mid().leftBound(), (it->second).rightBound()));
            tmp_m.insert(make_pair(it->first, tmp_v));
        }
        else
        {
            //cout << " no" << endl;
        }
    }
    return box_factory::cartesian_product(tmp_m);
}

/**
 * Dividing the box in all n dimensions producing 2^n boxes of the same size
 * according to the precision vector e
 */
vector<box> box_factory::bisect(box b, map<std::string, capd::interval> e)
{
    if(e.empty())
    {
        return {b};
    }
    std::map<std::string, std::vector<capd::interval>> tmp_m;
    std::map<std::string, capd::interval> m = b.get_map();
    for(auto it = m.cbegin(); it != m.cend(); it++)
    {
        if(capd::intervals::width(it->second) > e[it->first].leftBound())
        {
            std::vector<capd::interval> tmp_v;
            tmp_v.push_back(capd::interval((it->second).leftBound(), (it->second).mid().rightBound()));
            tmp_v.push_back(capd::interval((it->second).mid().leftBound(), (it->second).rightBound()));
            tmp_m.insert(make_pair(it->first, tmp_v));
        }
        else
        {
            tmp_m.insert(make_pair(it->first, vector<capd::interval>{it->second}));
        }
    }
    return box_factory::cartesian_product(tmp_m);
}

int accent_box::compute_metrics(int style)
{
  int r = p->compute_metrics(style);
  p->compute_skew();
  ab->compute_metrics(style);
  printf(".nr " LEFT_WIDTH_FORMAT " 0\\n[" WIDTH_FORMAT "]/2"
	 ">?(\\n[" WIDTH_FORMAT "]/2-\\n[" SKEW_FORMAT "])\n",
	 uid, p->uid, ab->uid, p->uid);
  printf(".nr " WIDTH_FORMAT " 0\\n[" WIDTH_FORMAT "]/2"
	 ">?(\\n[" WIDTH_FORMAT "]/2+\\n[" SKEW_FORMAT "])"
	 "+\\n[" LEFT_WIDTH_FORMAT "]\n",
	 uid, p->uid, ab->uid, p->uid, uid);
  printf(".nr " DEPTH_FORMAT " \\n[" DEPTH_FORMAT "]\n", uid, p->uid);
  printf(".nr " SUP_RAISE_FORMAT " \\n[" HEIGHT_FORMAT "]-%dM>?0\n",
	 uid, p->uid, x_height);
  printf(".nr " HEIGHT_FORMAT " \\n[" HEIGHT_FORMAT "]+\\n["
	 SUP_RAISE_FORMAT "]\n",
	 uid, ab->uid, uid);
  if (r)
    printf(".nr " MARK_REG " +\\n[" LEFT_WIDTH_FORMAT "]"
	   "-(\\n[" WIDTH_FORMAT "]/2)'\n",
	   uid, p->uid);
  return r;
}

 /*! Return true if \p a includes \p b. */
 friend pure bool includes(const sphere<T,N>& a,
                            const box<T,N>& b) {
     const auto n = b.corner_count();
     vec<T,N> c[n]; b.read_corners(c);
     return std::all_of(c,c+n, [a](vec<T,N>& p) { return a.includes(p); });
 }

double eval_enode_term(Enode * const e, box const & b) {
    if (e->isVar()) {
        return b.get_value(e).lb();
    } else if (e->isConstant()) {
        double const v = e->getValue();
        return v;
    } else if (e->isSymb()) {
        throw runtime_error("eval_enode: Symb");
    } else if (e->isNumb()) {
        throw runtime_error("eval_enode: Numb");
    } else if (e->isTerm()) {
        assert(e->getArity() >= 1);
        enodeid_t id = e->getCar()->getId();
        double ret = 0.0;
        Enode * tmp = e;
        switch (id) {
        case ENODE_ID_PLUS:
            ret = eval_enode_term(tmp->get1st(), b);
            tmp = tmp->getCdr()->getCdr();  // e is pointing to the 2nd arg
            while (!tmp->isEnil()) {
                ret = ret + eval_enode_term(tmp->getCar(), b);
                tmp = tmp->getCdr();
            }
            return ret;
        case ENODE_ID_MINUS:
            ret = eval_enode_term(tmp->get1st(), b);
            tmp = tmp->getCdr()->getCdr();  // e is pointing to the 2nd arg
            while (!tmp->isEnil()) {
                ret = ret - eval_enode_term(tmp->getCar(), b);
                tmp = tmp->getCdr();
            }
            return ret;
        case ENODE_ID_UMINUS:
            ret = eval_enode_term(tmp->get1st(), b);
            assert(tmp->getArity() == 1);
            return (- ret);
        case ENODE_ID_TIMES:
            ret = eval_enode_term(tmp->get1st(), b);
            tmp = tmp->getCdr()->getCdr();  // e is pointing to the 2nd arg
            while (!tmp->isEnil()) {
                ret = ret * eval_enode_term(tmp->getCar(), b);
                tmp = tmp->getCdr();
            }
            return ret;
        case ENODE_ID_DIV:
            ret = eval_enode_term(tmp->get1st(), b);
            tmp = tmp->getCdr()->getCdr();  // e is pointing to the 2nd arg
            while (!tmp->isEnil()) {
                ret = ret / eval_enode_term(tmp->getCar(), b);
                tmp = tmp->getCdr();
            }
            return ret;
        case ENODE_ID_ACOS:
            assert(e->getArity() == 1);
            return acos(eval_enode_term(e->get1st(), b));
        case ENODE_ID_ASIN:
            assert(e->getArity() == 1);
            return asin(eval_enode_term(e->get1st(), b));
        case ENODE_ID_ATAN:
            assert(e->getArity() == 1);
            return atan(eval_enode_term(e->get1st(), b));
        case ENODE_ID_ATAN2:
            assert(e->getArity() == 2);
            return atan2(eval_enode_term(e->get1st(), b),
                         eval_enode_term(e->get2nd(), b));
        case ENODE_ID_MIN:
            assert(e->getArity() == 2);
            return fmin(eval_enode_term(e->get1st(), b),
                        eval_enode_term(e->get2nd(), b));
        case ENODE_ID_MAX:
            assert(e->getArity() == 2);
            return fmax(eval_enode_term(e->get1st(), b),
                        eval_enode_term(e->get2nd(), b));
        case ENODE_ID_MATAN:
            assert(e->getArity() == 1);
            throw runtime_error("eval_enode: MATAN");
        case ENODE_ID_SAFESQRT:
            assert(e->getArity() == 1);
            throw runtime_error("eval_enode: SAFESQRT");
        case ENODE_ID_SQRT:
            assert(e->getArity() == 1);
            return sqrt(eval_enode_term(e->get1st(), b));
        case ENODE_ID_EXP:
            assert(e->getArity() == 1);
            return exp(eval_enode_term(e->get1st(), b));
        case ENODE_ID_LOG:
            assert(e->getArity() == 1);
            return log(eval_enode_term(e->get1st(), b));
        case ENODE_ID_POW:
            assert(e->getArity() == 2);
            return pow(eval_enode_term(e->get1st(), b),
                       eval_enode_term(e->get2nd(), b));
        case ENODE_ID_ABS:
            assert(e->getArity() == 1);
            return fabs(eval_enode_term(e->get1st(), b));
        case ENODE_ID_SIN:
            assert(e->getArity() == 1);
            return sin(eval_enode_term(e->get1st(), b));
        case ENODE_ID_COS:
            assert(e->getArity() == 1);
//.........这里部分代码省略.........

//collision move sphere with box
bool kgmCollision::collision(vec3& start, vec3& end, float radius, 
                             box& b, mtx4& btr)
{
  int i = 0;
  vec3 box_points[8];
  vec3 box_sides[6][4];
  b.points(box_points);

  for(i = 0; i < 8; i++)
    box_points[i] = btr * box_points[i];

  box_sides[0][0] = box_points[0];
  box_sides[0][1] = box_points[1];
  box_sides[0][2] = box_points[5];
  box_sides[0][3] = box_points[4];
  box_sides[1][0] = box_points[1];
  box_sides[1][1] = box_points[3];
  box_sides[1][2] = box_points[7];
  box_sides[1][3] = box_points[5];
  box_sides[2][0] = box_points[3];
  box_sides[2][1] = box_points[2];
  box_sides[2][2] = box_points[6];
  box_sides[2][3] = box_points[7];
  box_sides[3][0] = box_points[2];
  box_sides[3][1] = box_points[0];
  box_sides[3][2] = box_points[4];
  box_sides[3][3] = box_points[6];
  box_sides[4][0] = box_points[0];
  box_sides[4][1] = box_points[2];
  box_sides[4][2] = box_points[3];
  box_sides[4][3] = box_points[1];
  box_sides[5][0] = box_points[4];
  box_sides[5][1] = box_points[5];
  box_sides[5][2] = box_points[7];
  box_sides[5][3] = box_points[6];

  float dist = -1.0f;
  vec3  ptins;
  m_collision = false;

  for(i = 0; i < 6; i++)
  {
    if(collision(start, end, radius, box_sides[i], 4))
    {
      //  if(collision(start, end, radius, box_sides[i][0], box_sides[i][1],box_sides[i][2], ptins) ||
      //     collision(start, end, radius, box_sides[i][0], box_sides[i][2],box_sides[i][3], ptins)){
      ptins = m_point;
      m_collision = true;

      break;

      if(m_collision)
      {
        if(start.distance(m_point) > dist)
          m_point = ptins;
        else
          dist = start.distance(m_point);
      }
      else
      {
        dist = start.distance(m_point);
      }

      m_collision = true;
    }
  }

  return m_collision;
}

box intersect(box b1, box const & b2) {
    b1.intersect(b2);
    return b1;
}

void prime_box::output()
{
  p->output();
  pb->output();
}

box ncbt_icp::solve(box b, contractor & ctc, SMTConfig & config) {
    thread_local static unordered_set<shared_ptr<constraint>> used_constraints;
    used_constraints.clear();
    static unsigned prune_count = 0;
    thread_local static vector<box> box_stack;
    box_stack.clear();
    box_stack.push_back(b);
    do {
        // Loop Invariant
        DREAL_LOG_INFO << "ncbt_icp::solve - loop"
                       << "\t" << "box stack Size = " << box_stack.size();
        b = box_stack.back();
        try {
            ctc.prune(b, config);
            auto const this_used_constraints = ctc.used_constraints();
            used_constraints.insert(this_used_constraints.begin(), this_used_constraints.end());
            if (config.nra_use_stat) { config.nra_stat.increase_prune(); }
        } catch (contractor_exception & e) {
            // Do nothing
        }
        prune_count++;
        box_stack.pop_back();
        if (!b.is_empty()) {
            // SAT
            tuple<int, box, box> splits = b.bisect(config.nra_precision);
            if (config.nra_use_stat) { config.nra_stat.increase_branch(); }
            int const index = get<0>(splits);
            if (index >= 0) {
                box const & first    = get<1>(splits);
                box const & second   = get<2>(splits);
                assert(first.get_idx_last_branched() == index);
                assert(second.get_idx_last_branched() == index);
                if (second.is_bisectable()) {
                    box_stack.push_back(second);
                    box_stack.push_back(first);
                } else {
                    box_stack.push_back(first);
                    box_stack.push_back(second);
                }
            } else {
                break;
            }
        } else {
            // UNSAT (b is emptified by pruning operators)
            // If this bisect_var is not used in all used
            // constraints, this box is safe to be popped.
            thread_local static unordered_set<Enode *> used_vars;
            used_vars.clear();
            for (auto used_ctr : used_constraints) {
                auto this_used_vars = used_ctr->get_vars();
                used_vars.insert(this_used_vars.begin(), this_used_vars.end());
            }
            while (box_stack.size() > 0) {
                int const bisect_var = box_stack.back().get_idx_last_branched();
                assert(bisect_var >= 0);
                // If this bisect_var is not used in all used
                // constraints, this box is safe to be popped.
                if (used_vars.find(b.get_vars()[bisect_var]) != used_vars.end()) {
                    // DREAL_LOG_FATAL << b.get_vars()[bisect_var] << " is used in "
                    //                 << *used_ctr << " and it's not safe to skip";
                    break;
                }
                // DREAL_LOG_FATAL << b.get_vars()[bisect_var] << " is not used and it's safe to skip this box"
                //                 << " (" << box_stack.size() << ")";
                box_stack.pop_back();
            }
        }
    } while (box_stack.size() > 0);
    DREAL_LOG_DEBUG << "prune count = " << prune_count;
    ctc.set_used_constraints(used_constraints);
    return b;
}

void contractor_gsl::prune(box & b, SMTConfig & config) {
    // TODO(soonhok): add timeout
    fesetround(FE_TONEAREST);  // Without this, GSL might cause a segmentation fault due to problems in floating point lib
    gsl_odeiv2_step_reset(m_step);
    gsl_odeiv2_evolve_reset(m_evolve);

    double const T_lb = b[m_time_t].lb();
    double const T_ub = b[m_time_t].ub();
    double t = 0.0, old_t = 0.0;                  /* initialize t */
    double T_next = 0.0;
    double h = 1e-10;              /* starting step size for ode solver */
    DREAL_LOG_INFO << "GSL: prune begin "
                   << m_time_t << " = ["
                   << T_lb << ", " << T_ub << "]"
                   << "\t" << b.max_diam();
    DREAL_LOG_INFO << m_ctr->get_ic();

    if (b.max_diam() < config.nra_precision) {
        return;
    }
    bool need_to_run = false;
    for (Enode * e : m_vars_0) {
        if (b[e].diam() > config.nra_precision) {
            need_to_run = true;
            break;
        }
    }
    if (b[m_time_t].diam() > config.nra_precision) {
        need_to_run = true;
    }
    if (!need_to_run) { return; }

    extract_sample_point(b, m_vars_0, m_values);
    extract_sample_point(b, m_pars_0, m_params);
    for (unsigned i = 0; i < m_vars_0.size(); i++) {
        b[m_vars_0[i]] = m_values[i];
    }
    for (unsigned i = 0; i < m_pars_0.size(); i++) {
        b[m_pars_0[i]] = m_params[i];
    }

    // First move to T_lb without checking m_values
    while (t < T_lb) {
        interruption_point();
        T_next = T_lb;
        // T_next = min(t + config.nra_precision, T_lb);
        int status = gsl_odeiv2_evolve_apply(m_evolve, m_control, m_step,
                                             &m_system,
                                             &t, T_next,
                                             &h, m_values);
        if (status != GSL_SUCCESS) {
            DREAL_LOG_INFO << "GSL: error, return value " << status;
            throw contractor_exception("GSL FAILED");
        }
    }

    // Now we're in the range in [T_lb, T_ub], need to check m_values.
    while (t < T_ub) {
        interruption_point();
        T_next = min(t + config.nra_precision, T_ub);
        // T_next = T_ub;

        // Copy m_values to m_old_values, and t to old_t
        for (unsigned i = 0; i < m_dim; i++) {
            m_old_values[i] = m_values[i];
        }
        old_t = t;
        int status = gsl_odeiv2_evolve_apply(m_evolve, m_control, m_step,
                                             &m_system,
                                             &t, T_next,
                                             &h, m_values);
        if (status != GSL_SUCCESS) {
            DREAL_LOG_INFO << "GSL: error, return value " << status;
            throw contractor_exception("GSL FAILED");
        }

        // print_values(t, m_values, m_dim);         /* print at t */
        bool values_good = true;
        unsigned i = 0;
        for (Enode * e : m_vars_t) {
            double const old_v_i = m_old_values[i];
            double const v_i = m_values[i];
            auto iv = (old_v_i < v_i) ? ibex::Interval(old_v_i, v_i) : ibex::Interval(v_i, old_v_i);
            auto const & iv_X_t = b[e];
            iv &= iv_X_t;
            if (iv.is_empty()) {
                values_good = false;
                DREAL_LOG_INFO << "GSL Not in Range: " << e
                                << " : " << m_values[i] << " not in " << b[e] << " at t = " << t;
                break;
            }
            i++;
        }
        if (values_good) {
            thread_local static box old_box(b);
            old_box = b;
            // Update X_t with m_values
            i = 0;
            for (Enode * e : m_vars_t) {
                double const old_v_i = m_old_values[i];
//.........这里部分代码省略.........

box naive_icp::solve(box b, contractor & ctc, SMTConfig & config) {
    thread_local static std::unordered_set<std::shared_ptr<constraint>> used_constraints;
    used_constraints.clear();
    thread_local static vector<box> solns;
    thread_local static vector<box> box_stack;
    solns.clear();
    box_stack.clear();
    box_stack.push_back(b);
    do {
        DREAL_LOG_INFO << "naive_icp::solve - loop"
                       << "\t" << "box stack Size = " << box_stack.size();
        b = box_stack.back();
        box_stack.pop_back();
        try {
            ctc.prune(b, config);
            auto this_used_constraints = ctc.used_constraints();
            used_constraints.insert(this_used_constraints.begin(), this_used_constraints.end());
            if (config.nra_use_stat) { config.nra_stat.increase_prune(); }
        } catch (contractor_exception & e) {
            // Do nothing
        }
        if (!b.is_empty()) {
            tuple<int, box, box> splits = b.bisect(config.nra_precision);
            if (config.nra_use_stat) { config.nra_stat.increase_branch(); }
            int const i = get<0>(splits);
            if (i >= 0) {
                box const & first  = get<1>(splits);
                box const & second = get<2>(splits);
                assert(first.get_idx_last_branched() == i);
                assert(second.get_idx_last_branched() == i);
                if (second.is_bisectable()) {
                    box_stack.push_back(second);
                    box_stack.push_back(first);
                } else {
                    box_stack.push_back(first);
                    box_stack.push_back(second);
                }
                if (config.nra_proof) {
                    config.nra_proof_out << "[branched on "
                                         << b.get_name(i)
                                         << "]" << endl;
                }
            } else {
                config.nra_found_soln++;
                if (config.nra_multiple_soln > 1) {
                    // If --multiple_soln is used
                    output_solution(b, config, config.nra_found_soln);
                }
                if (config.nra_found_soln >= config.nra_multiple_soln) {
                    break;
                }
                solns.push_back(b);
            }
        }
    } while (box_stack.size() > 0);
    ctc.set_used_constraints(used_constraints);
    if (config.nra_multiple_soln > 1 && solns.size() > 0) {
        return solns.back();
    } else {
        assert(!b.is_empty() || box_stack.size() == 0);
        return b;
    }
}

void delim_box::check_tabs(int level)
{
  p->check_tabs(level);
}

void uaccent_box::check_tabs(int level)
{
  ab->check_tabs(level + 1);
  p->check_tabs(level + 1);
}

int script_box::compute_metrics(int style)
{
  int res = p->compute_metrics(style);
  p->compute_subscript_kern();
  printf(".nr " SIZE_FORMAT " \\n[.ps]\n", uid);
  if (!(style <= SCRIPT_STYLE && one_size_reduction_flag))
    set_script_size();
  printf(".nr " SMALL_SIZE_FORMAT " \\n[.ps]\n", uid);
  if (sub != 0)
    sub->compute_metrics(cramped_style(script_style(style)));
  if (sup != 0)
    sup->compute_metrics(script_style(style));
  // 18a
  if (p->is_char()) {
    printf(".nr " SUP_RAISE_FORMAT " 0\n", uid);
    printf(".nr " SUB_LOWER_FORMAT " 0\n", uid);
  }
  else {
    printf(".nr " SUP_RAISE_FORMAT " \\n[" HEIGHT_FORMAT "]-%dM>?0\n",
	   uid, p->uid, sup_drop);
    printf(".nr " SUB_LOWER_FORMAT " \\n[" DEPTH_FORMAT "]+%dM\n",
	   uid, p->uid, sub_drop);
  }
  printf(".ps \\n[" SIZE_FORMAT "]u\n", uid);
  if (sup == 0) {
    assert(sub != 0);
    // 18b
    printf(".nr " SUB_LOWER_FORMAT " \\n[" SUB_LOWER_FORMAT "]>?%dM>?(\\n["
	   HEIGHT_FORMAT "]-(%dM*4/5))\n",
	   uid, uid, sub1, sub->uid, x_height);
  }
  else {
    // sup != 0
    // 18c
    int pos;
    if (style == DISPLAY_STYLE)
      pos = sup1;
    else if (style & 1)		// not cramped
      pos = sup2;
    else
      pos = sup3;
    printf(".nr " SUP_RAISE_FORMAT " \\n[" SUP_RAISE_FORMAT
	   "]>?%dM>?(\\n[" DEPTH_FORMAT "]+(%dM/4))\n",
	   uid, uid, pos, sup->uid, x_height);
    // 18d
    if (sub != 0) {
      printf(".nr " SUB_LOWER_FORMAT " \\n[" SUB_LOWER_FORMAT "]>?%dM\n",
	     uid, uid, sub2);
      // 18e
      printf(".nr " TEMP_REG " \\n[" DEPTH_FORMAT "]-\\n["
	     SUP_RAISE_FORMAT "]+\\n[" HEIGHT_FORMAT "]-\\n["
	     SUB_LOWER_FORMAT "]+(4*%dM)\n",
	     sup->uid, uid, sub->uid, uid, default_rule_thickness);
      printf(".if \\n[" TEMP_REG "] \\{");
      printf(".nr " SUB_LOWER_FORMAT " +\\n[" TEMP_REG "]\n", uid);
      printf(".nr " TEMP_REG " (%dM*4/5)-\\n[" SUP_RAISE_FORMAT
	     "]+\\n[" DEPTH_FORMAT "]>?0\n",
	     x_height, uid, sup->uid);
      printf(".nr " SUP_RAISE_FORMAT " +\\n[" TEMP_REG "]\n", uid);
      printf(".nr " SUB_LOWER_FORMAT " -\\n[" TEMP_REG "]\n", uid);
      printf(".\\}\n");
    }
  }
  printf(".nr " WIDTH_FORMAT " 0\\n[" WIDTH_FORMAT "]", uid, p->uid);
  if (sub != 0 && sup != 0)
    printf("+((\\n[" WIDTH_FORMAT "]-\\n[" SUB_KERN_FORMAT "]>?\\n["
	   WIDTH_FORMAT "])+%dM)>?0\n",
	   sub->uid, p->uid, sup->uid, script_space);
  else if (sub != 0)
    printf("+(\\n[" WIDTH_FORMAT "]-\\n[" SUB_KERN_FORMAT "]+%dM)>?0\n",
	   sub->uid, p->uid, script_space);
  else if (sup != 0)
    printf("+(\\n[" WIDTH_FORMAT "]+%dM)>?0\n", sup->uid, script_space);
  else
    printf("\n");
  printf(".nr " HEIGHT_FORMAT " \\n[" HEIGHT_FORMAT "]",
	 uid, p->uid);
  if (sup != 0)
    printf(">?(\\n[" SUP_RAISE_FORMAT "]+\\n[" HEIGHT_FORMAT "])",
	   uid, sup->uid);
  if (sub != 0)
    printf(">?(-\\n[" SUB_LOWER_FORMAT "]+\\n[" HEIGHT_FORMAT "])",
	   uid, sub->uid);
  printf("\n");
  printf(".nr " DEPTH_FORMAT " \\n[" DEPTH_FORMAT "]",
	 uid, p->uid);
  if (sub != 0)
    printf(">?(\\n[" SUB_LOWER_FORMAT "]+\\n[" DEPTH_FORMAT "])",
	   uid, sub->uid);
  if (sup != 0)
    printf(">?(-\\n[" SUP_RAISE_FORMAT "]+\\n[" DEPTH_FORMAT "])",
	   uid, sup->uid);
  printf("\n");
  return res;
}

contractor_sample::contractor_sample(box const & b, unsigned const n, vector<shared_ptr<constraint>> const & ctrs)
    : contractor_cell(contractor_kind::SAMPLE), m_num_samples(n), m_ctrs(ctrs) {
    m_input = ibex::BitSet::all(b.size());
}