<commit_before>/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/*
 * @file
 */

#include "modules/planning/open_space/coarse_trajectory_generator/grid_search.h"

namespace apollo {
namespace planning {

GridSearch::GridSearch(const PlannerOpenSpaceConfig& open_space_conf) {
  xy_grid_resolution_ =
      open_space_conf.warm_start_config().grid_a_star_xy_resolution();
  node_radius_ = open_space_conf.warm_start_config().node_radius();
}

double GridSearch::EuclidDistance(const double x1, const double y1,
                                  const double x2, const double y2) {
  return std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
  if (obstacles_linesegments_vec_.empty()) {
    return true;
  }
  double node_grid_x = node->GetGridX();
  double node_grid_y = node->GetGridY();
  if (node_grid_x > max_grid_x_ || node_grid_x < 0 ||
      node_grid_y > max_grid_y_ || node_grid_y < 0) {
    return false;
  }
  for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
    for (const common::math::LineSegment2d& linesegment :
         obstacle_linesegments) {
      if (linesegment.DistanceTo({node->GetGridX(), node->GetGridY()}) <
          node_radius_) {
        return false;
      }
    }
  }
  return true;
}

std::vector<std::shared_ptr<Node2d>> GridSearch::GenerateNextNodes(
    std::shared_ptr<Node2d> current_node) {
  double current_node_x = current_node->GetGridX();
  double current_node_y = current_node->GetGridY();
  double current_node_path_cost = current_node->GetPathCost();
  double diagonal_distance = std::sqrt(2.0);
  std::vector<std::shared_ptr<Node2d>> next_nodes;
  std::shared_ptr<Node2d> up =
      std::make_shared<Node2d>(current_node_x, current_node_y + 1.0, XYbounds_);
  up->SetPathCost(current_node_path_cost + 1.0);<commit_msg>Planning: fix a potential bug in grid search<commit_after>/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/*
 * @file
 */

#include "modules/planning/open_space/coarse_trajectory_generator/grid_search.h"

namespace apollo {
namespace planning {

GridSearch::GridSearch(const PlannerOpenSpaceConfig& open_space_conf) {
  xy_grid_resolution_ =
      open_space_conf.warm_start_config().grid_a_star_xy_resolution();
  node_radius_ = open_space_conf.warm_start_config().node_radius();
}

double GridSearch::EuclidDistance(const double x1, const double y1,
                                  const double x2, const double y2) {
  return std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
  const double node_grid_x = node->GetGridX();
  const double node_grid_y = node->GetGridY();
  if (node_grid_x > max_grid_x_ || node_grid_x < 0 ||
      node_grid_y > max_grid_y_ || node_grid_y < 0) {
    return false;
  }
  if (obstacles_linesegments_vec_.empty()) {
    return true;
  }
  for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
    for (const common::math::LineSegment2d& linesegment :
         obstacle_linesegments) {
      if (linesegment.DistanceTo({node->GetGridX(), node->GetGridY()}) <
          node_radius_) {
        return false;
      }
    }
  }
  return true;
}

std::vector<std::shared_ptr<Node2d>> GridSearch::GenerateNextNodes(
    std::shared_ptr<Node2d> current_node) {
  double current_node_x = current_node->GetGridX();
  double current_node_y = current_node->GetGridY();
  double current_node_path_cost = current_node->GetPathCost();
  double diagonal_distance = std::sqrt(2.0);
  std::vector<std::shared_ptr<Node2d>> next_nodes;
  std::shared_ptr<Node2d> up =
      std::make_shared<Node2d>(current_node_x, current_node_y + 1.0, XYbounds_);
  up->SetPathCost(current_node_path_cost + 1.0);
{'fraction_non_alphanumeric': 0.16222945795824556, 'fraction_numerical': 0.013407393219689714, 'mean_word_length': 4.496842105263158, 'pattern_counts': {'":': 0, '<': 25, '<?xml version=': 0, '>': 39, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3021568', 'n_tokens_mistral': 1646, 'n_tokens_neox': 1589, 'n_words': 390}