<commit_before>#pragma once

#include <Geometry2d/Point.hpp>
#include <rrt/2dplane/PlaneStateSpace.hpp>

namespace Planning {

/**
 * Represents the robocup field for path-planning purposes.
 */
class RoboCupStateSpace : public RRT::StateSpace<Geometry2d::Point> {
public:
    RoboCupStateSpace(const Field_Dimensions& dims,
                      const Geometry2d::ShapeSet& obstacles)
        : _fieldDimensions(dims), _obstacles(obstacles) {}

    Geometry2d::Point randomState() const {
        float x = _fieldDimensions.FloorWidth() * (drand48() - 0.5f);
        float y = _fieldDimensions.FloorLength() * drand48() -
                  _fieldDimensions.Border();
        return Geometry2d::Point(x, y);
    }

    double distance(const Geometry2d::Point& from,
                    const Geometry2d::Point& to) const {
        return from.distTo(to);
    }

    bool stateValid(const Geometry2d::Point& state) const {
        // note: _obstacles contains obstacles that define the limits of the
        // field, so we shouldn't have to check separately that the point is
        // within the field boundaries.

        return !_obstacles.hit(state);
    }

    Geometry2d::Point intermediateState(const Geometry2d::Point& source,
                                        const Geometry2d::Point& target,
                                        float stepSize) const {
        auto dir = (target - source).norm();
        return source + dir * stepSize;
    }

    Geometry2d::Point intermediateState(const Geometry2d::Point& source,
                                        const Geometry2d::Point& target,
                                        float minStepSize,
                                        float maxStepSize) const {
        throw std::runtime_error("Adaptive stepsize control not implemented");
    }

    bool transitionValid(const Geometry2d::Point& from,
                         const Geometry2d::Point& to) const {
        // TODO: what if it starts out in an obstacle?
        if (_obstacles.hit(Geometry2d::Segment(from, to))) return false;
        return true;
    }

private:
    const Geometry2d::ShapeSet& _obstacles;
    const Field_Dimensions _fieldDimensions;
};

}  // namespace Planning<commit_msg>rrt: allow robot to start inside an obstacle<commit_after>#pragma once

#include <Geometry2d/Point.hpp>
#include <rrt/2dplane/PlaneStateSpace.hpp>

namespace Planning {

/**
 * Represents the robocup field for path-planning purposes.
 */
class RoboCupStateSpace : public RRT::StateSpace<Geometry2d::Point> {
public:
    RoboCupStateSpace(const Field_Dimensions& dims,
                      const Geometry2d::ShapeSet& obstacles)
        : _fieldDimensions(dims), _obstacles(obstacles) {}

    Geometry2d::Point randomState() const {
        float x = _fieldDimensions.FloorWidth() * (drand48() - 0.5f);
        float y = _fieldDimensions.FloorLength() * drand48() -
                  _fieldDimensions.Border();
        return Geometry2d::Point(x, y);
    }

    double distance(const Geometry2d::Point& from,
                    const Geometry2d::Point& to) const {
        return from.distTo(to);
    }

    bool stateValid(const Geometry2d::Point& state) const {
        // note: _obstacles contains obstacles that define the limits of the
        // field, so we shouldn't have to check separately that the point is
        // within the field boundaries.

        return !_obstacles.hit(state);
    }

    Geometry2d::Point intermediateState(const Geometry2d::Point& source,
                                        const Geometry2d::Point& target,
                                        float stepSize) const {
        auto dir = (target - source).norm();
        return source + dir * stepSize;
    }

    Geometry2d::Point intermediateState(const Geometry2d::Point& source,
                                        const Geometry2d::Point& target,
                                        float minStepSize,
                                        float maxStepSize) const {
        throw std::runtime_error("Adaptive stepsize control not implemented");
    }

    bool transitionValid(const Geometry2d::Point& from,
                         const Geometry2d::Point& to) const {
        // Ensure that @to doesn't hit any obstacles that @from doesn't. This
        // allows the RRT to start inside an obstacle, but prevents it from
        // entering a new obstacle.
        for (const auto& shape : _obstacles.shapes()) {
            if (shape->hit(Segment(from, to)) && !shape->hit(from))
                return false;
        }
        return true;
    }

private:
    const Geometry2d::ShapeSet& _obstacles;
    const Field_Dimensions _fieldDimensions;
};

}  // namespace Planning
{'fraction_non_alphanumeric': 0.0924476412100698, 'fraction_numerical': 0.010365982652845357, 'mean_word_length': 2.143617021276596, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 10, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4922994', 'n_tokens_mistral': 1297, 'n_tokens_neox': 1225, 'n_words': 342}