<commit_before>// Copyright (c) 2014, LAAS-CNRS
// Authors: <NAME> (<EMAIL>)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include "hpp/core/path-projector/progressive.hh"

#include <hpp/core/path-vector.hh>
#include <hpp/core/config-projector.hh>

#include <limits>
#include <queue>
#include <stack>

namespace hpp {
  namespace core {
    namespace pathProjector {
      Progressive::Progressive (const core::DistancePtr_t d, value_type step) :
        PathProjector (d), step_ (step)
      {}

      bool Progressive::impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const
      {
        ConstraintSetPtr_t constraints = path->constraints ();
        const ConfigProjectorPtr_t& cp = constraints->configProjector ();
        const StraightPath& sp = *path;
        core::interval_t timeRange = sp.timeRange ();
        const Configuration_t& q1 = sp(timeRange.first);
        const Configuration_t& q2 = sp(timeRange.second);
        if (cp) cp->rightHandSideFromConfig(q1);
        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
          return false;
        }
        if (!cp) {
          projection = path;
          return true;
        }

        bool pathIsFullyProjected = false;
        std::queue <core::StraightPathPtr_t> paths;
        StraightPathPtr_t toSplit =
            core::StraightPath::create (sp.device (), q1, q2, d (q1, q2));
        Configuration_t qi (q1.size());
        value_type curStep, curLength;
        size_t c = 0;
        while (true) {
          const StraightPath& toSplitRef = *toSplit;
          if (toSplitRef.length () < step_) {
            paths.push (toSplit);
            pathIsFullyProjected = true;
            break;
          }
          timeRange = toSplitRef.timeRange ();
          const Configuration_t& qb = toSplitRef (timeRange.first);
          curStep = step_;
          curLength = std::numeric_limits <value_type>::max();
          bool stop = false;
          /// Find the good length.
          /// Here, it would be good to have an upper bound of the Hessian
          /// of the constraint.
          do {
            if (curStep < 1e-2) {
              stop = true;
              break;
            }
            toSplitRef (qi, curStep);
            if (constraints->apply (qi)) {
              curLength = d (qb, qi);
            }
            curStep /= 2;
          } while (curLength > step_ ||
              curLength < 1e-2);
          if (stop) break;
          if (c > 200) break;
          StraightPathPtr_t part =
            core::StraightPath::create (sp.device (), qb, qi, curLength);
          paths.push (part);
          toSplit =
            core::StraightPath::create (sp.device (), qi, q2, d (qi, q2));
          c++;
        }
        switch (paths.size ()) {
          case 0:
            timeRange = sp.timeRange();
            projection = sp.extract (std::make_pair (timeRange.first, timeRange.first));
            return false;
            break;
          case 1:
            projection = paths.front ();
            projection->constraints (constraints);
            break;<commit_msg>Enhance break conditions in pathProjector::Progressive.<commit_after>// Copyright (c) 2014, LAAS-CNRS
// Authors: <NAME> (<EMAIL>)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include "hpp/core/path-projector/progressive.hh"

#include <hpp/core/path-vector.hh>
#include <hpp/core/config-projector.hh>

#include <limits>
#include <queue>
#include <stack>

namespace hpp {
  namespace core {
    namespace pathProjector {
      Progressive::Progressive (const core::DistancePtr_t d, value_type step) :
        PathProjector (d), step_ (step)
      {}

      bool Progressive::impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const
      {
        ConstraintSetPtr_t constraints = path->constraints ();
        const ConfigProjectorPtr_t& cp = constraints->configProjector ();
        const StraightPath& sp = *path;
        core::interval_t timeRange = sp.timeRange ();
        const Configuration_t& q1 = sp(timeRange.first);
        const Configuration_t& q2 = sp(timeRange.second);
        const size_t maxDichotomyTries = 10,
                     maxPathSplit = (size_t)(10 * (timeRange.second - timeRange.first) / (double)step_);
        if (cp) cp->rightHandSideFromConfig(q1);
        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
          return false;
        }
        if (!cp) {
          projection = path;
          return true;
        }

        bool pathIsFullyProjected = false;
        std::queue <core::StraightPathPtr_t> paths;
        StraightPathPtr_t toSplit =
            core::StraightPath::create (sp.device (), q1, q2, d (q1, q2));
        Configuration_t qi (q1.size());
        value_type curStep, curLength;
        size_t c = 0;
        while (true) {
          const StraightPath& toSplitRef = *toSplit;
          if (toSplitRef.length () < step_) {
            paths.push (toSplit);
            pathIsFullyProjected = true;
            break;
          }
          timeRange = toSplitRef.timeRange ();
          const Configuration_t& qb = toSplitRef (timeRange.first);
          curStep = step_;
          curLength = std::numeric_limits <value_type>::max();
          size_t dicC = 0;
          /// Find the good length.
          /// Here, it would be good to have an upper bound of the Hessian
          /// of the constraint.
          do {
            if (dicC >= maxDichotomyTries) break;
            toSplitRef (qi, curStep);
            if (constraints->apply (qi))
              curLength = d (qb, qi);
            curStep /= 2;
            dicC++;
          } while (curLength > step_ || curLength < 1e-3);
          if (dicC >= maxDichotomyTries || c > maxPathSplit) break;
          StraightPathPtr_t part =
            core::StraightPath::create (sp.device (), qb, qi, curLength);
          paths.push (part);
          toSplit =
            core::StraightPath::create (sp.device (), qi, q2, d (qi, q2));
          c++;
        }
        switch (paths.size ()) {
          case 0:
            timeRange = sp.timeRange();
            projection = sp.extract (std::make_pair (timeRange.first, timeRange.first));
            return false;
            break;
          case 1:
            projection = paths.front ();
            projection->constraints (constraints);
            break;
{'fraction_non_alphanumeric': 0.11002731174404994, 'fraction_numerical': 0.0072831317466510595, 'mean_word_length': 2.1247460381958554, 'pattern_counts': {'":': 0, '<': 28, '<?xml version=': 0, '>': 43, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '952696', 'n_tokens_mistral': 2247, 'n_tokens_neox': 2184, 'n_words': 670}