<commit_before>/******************************************************************************
 * Copyright 2017 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.
 *****************************************************************************/

/*
 * spiral_reference_line_smoother.cc
 */

#include "modules/planning/reference_line/spiral_reference_line_smoother.h"

#include <algorithm>
#include <iomanip>
#include <utility>

#include "IpIpoptApplication.hpp"
#include "IpSolveStatistics.hpp"

#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quintic_spiral_path.h"
#include "modules/planning/reference_line/spiral_problem_interface.h"

namespace apollo {
namespace planning {

using apollo::common::time::Clock;

SpiralReferenceLineSmoother::SpiralReferenceLineSmoother(
    const double max_point_deviation)
    : default_max_point_deviation_(max_point_deviation) {
  CHECK(max_point_deviation >= 0.0);
}

bool SpiralReferenceLineSmoother::Smooth(
    const ReferenceLine& raw_reference_line,
    ReferenceLine* const smoothed_reference_line) {
  const double start_timestamp = Clock::NowInSeconds();
  std::vector<double> opt_x;
  std::vector<double> opt_y;
  std::vector<double> opt_theta;
  std::vector<double> opt_kappa;
  std::vector<double> opt_dkappa;
  std::vector<double> opt_s;

  if (anchor_points_.empty()) {
    const double piecewise_length = FLAGS_spiral_smoother_piecewise_length;
    const double length = raw_reference_line.Length();
    ADEBUG << "Length = " << length;
    uint32_t num_of_pieces =
        std::max(1u, static_cast<uint32_t>(length / piecewise_length));

    const double delta_s = length / num_of_pieces;
    double s = 0.0;

    std::vector<Eigen::Vector2d> raw_point2d;
    for (std::uint32_t i = 0; i <= num_of_pieces;
         ++i, s = std::fmin(s + delta_s, length)) {
      ReferencePoint rlp = raw_reference_line.GetReferencePoint(s);
      raw_point2d.emplace_back(rlp.x(), rlp.y());
    }

    Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
           &opt_y);
  } else {
    std::size_t start_index = 0;
    for (const auto& anchor_point : anchor_points_) {
      if (anchor_point.enforced) {
        start_index++;
      } else {
        break;
      }
    }

    std::vector<Eigen::Vector2d> raw_point2d;
    if (start_index == 0) {
      for (const auto& anchor_point : anchor_points_) {
        raw_point2d.emplace_back(anchor_point.path_point.x(),
                                 anchor_point.path_point.y());
      }
    } else {
      std::vector<double> overhead_s;
      for (std::size_t i = 0; i + 1 < start_index; ++i) {
        const auto& p0 = anchor_points_[i];
        const auto& p1 = anchor_points_[i + 1];
        overhead_s.push_back(p1.path_point.s() - p0.path_point.s());
      }

      std::vector<double> overhead_theta;
      std::vector<double> overhead_kappa;
      std::vector<double> overhead_dkappa;
      std::vector<double> overhead_x;
      std::vector<double> overhead_y;
      for (std::size_t i = 0; i < anchor_points_.size(); ++i) {
        const auto& p = anchor_points_[i];
        if (i + 1 < start_index) {
          overhead_theta.push_back(p.path_point.theta());
          overhead_kappa.push_back(p.path_point.kappa());
          overhead_dkappa.push_back(p.path_point.dkappa());
          overhead_x.push_back(p.path_point.x());
          overhead_y.push_back(p.path_point.y());
        } else {
          raw_point2d.emplace_back(p.path_point.x(), p.path_point.y());
        }
      }

      fixed_start_point_ = true;
      fixed_start_x_ = anchor_points_[start_index - 1].path_point.x();
      fixed_start_y_ = anchor_points_[start_index - 1].path_point.y();
      fixed_start_theta_ = common::math::NormalizeAngle(
          anchor_points_[start_index - 1].path_point.theta());
      fixed_start_kappa_ = anchor_points_[start_index - 1].path_point.kappa();
      fixed_start_dkappa_ = anchor_points_[start_index - 1].path_point.dkappa();

      Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
             &opt_y);

      opt_theta.insert(opt_theta.begin(), overhead_theta.begin(),
                       overhead_theta.end());
      opt_kappa.insert(opt_kappa.begin(), overhead_kappa.begin(),
                       overhead_kappa.end());
      opt_dkappa.insert(opt_dkappa.begin(), overhead_dkappa.begin(),
                        overhead_dkappa.end());
      opt_s.insert(opt_s.begin(), overhead_s.begin(), overhead_s.end());
      opt_x.insert(opt_x.begin(), overhead_x.begin(), overhead_x.end());
      opt_y.insert(opt_y.begin(), overhead_y.begin(), overhead_y.end());

      std::for_each(opt_x.begin(), opt_x.end(), [this](double& x){
        x += zero_x_;
      });

      std::for_each(opt_y.begin(), opt_y.end(), [this](double& y){
        y += zero_y_;
      });
    }
  }

  std::vector<common::PathPoint> smoothed_point2d =
      Interpolate(opt_theta, opt_kappa, opt_dkappa, opt_s, opt_x, opt_y,
                  FLAGS_spiral_reference_line_resolution);

  std::vector<ReferencePoint> ref_points;
  for (const auto& p : smoothed_point2d) {
    const double heading = p.theta();
    const double kappa = p.kappa();
    const double dkappa = p.dkappa();

    common::SLPoint ref_sl_point;
    if (!raw_reference_line.XYToSL({p.x(), p.y()}, &ref_sl_point)) {
      return false;
    }
    if (ref_sl_point.s() < 0 ||
        ref_sl_point.s() > raw_reference_line.Length()) {
      continue;
    }

    ReferencePoint rlp = raw_reference_line.GetReferencePoint(ref_sl_point.s());
    ref_points.emplace_back(
        ReferencePoint(hdmap::MapPathPoint(common::math::Vec2d(p.x(), p.y()),
                                           heading, rlp.lane_waypoints()),
                       kappa, dkappa, 0.0, 0.0));
  }

  ReferencePoint::RemoveDuplicates(&ref_points);
  if (ref_points.size() < 2) {
    AERROR << "Fail to generate smoothed reference line.";
    return false;
  }
  *smoothed_reference_line = ReferenceLine(ref_points);
  const double end_timestamp = Clock::NowInSeconds();
  ADEBUG << "Spiral reference line smoother time: "
         << (end_timestamp - start_timestamp) * 1000 << " ms.";
  return true;
}

bool SpiralReferenceLineSmoother::Smooth(std::vector<Eigen::Vector2d> point2d,
                                         std::vector<double>* ptr_theta,
                                         std::vector<double>* ptr_kappa,
                                         std::vector<double>* ptr_dkappa,
                                         std::vector<double>* ptr_s,
                                         std::vector<double>* ptr_x,
                                         std::vector<double>* ptr_y) const {
  CHECK_GT(point2d.size(), 1);

  SpiralProblemInterface* ptop = new SpiralProblemInterface(point2d);
  ptop->set_default_max_point_deviation(default_max_point_deviation_);
  if (fixed_start_point_) {
    ptop->set_start_point(fixed_start_x_, fixed_start_y_, fixed_start_theta_,
                          fixed_start_kappa_, fixed_start_dkappa_);
  }

  Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;

  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();

  app->Options()->SetStringValue("hessian_approximation", "limited-memory");
  //  app->Options()->SetStringValue("derivative_test", "first-order");
  //  app->Options()->SetNumericValue("derivative_test_perturbation", 1.0e-7);
  //  app->Options()->SetStringValue("derivative_test", "second-order");
  app->Options()->SetIntegerValue("print_level", 0);
  int num_iterations = FLAGS_spiral_smoother_num_iteration;
  app->Options()->SetIntegerValue("max_iter", num_iterations);
  app->Options()->SetIntegerValue("acceptable_iter", 5);
  app->Options()->SetNumericValue("tol", 1.0e-3);
  app->Options()->SetNumericValue("acceptable_tol", 1.0e-3);

  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    ADEBUG << "*** Error during initialization!";
    return static_cast<int>(status);
  }

  status = app->OptimizeTNLP(problem);

  if (status == Ipopt::Solve_Succeeded ||
      status == Ipopt::Solved_To_Acceptable_Level) {
    // Retrieve some statistics about the solve
    Ipopt::Index iter_count = app->Statistics()->IterationCount();
    ADEBUG << "*** The problem solved in " << iter_count << " iterations!";

    Ipopt::Number final_obj = app->Statistics()->FinalObjective();
    ADEBUG << "*** The final value of the objective function is " << final_obj
           << '.';
  } else {
    ADEBUG << "Return status: " << int(status);
  }

  ptop->get_optimization_results(ptr_theta, ptr_kappa, ptr_dkappa, ptr_s, ptr_x,
                                 ptr_y);

  return status == Ipopt::Solve_Succeeded ||
         status == Ipopt::Solved_To_Acceptable_Level;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const std::vector<double>& theta, const std::vector<double>& kappa,
    const std::vector<double>& dkappa, const std::vector<double>& s,
    const std::vector<double>& x, const std::vector<double>& y,
    const double resolution) const {
  std::vector<common::PathPoint> smoothed_point2d;
  double start_s = 0.0;
  common::PathPoint first_point =
      to_path_point(x.front(), y.front(), start_s, theta.front(), kappa.front(),
                    dkappa.front());
  smoothed_point2d.push_back(first_point);

  for (std::size_t i = 0; i + 1 < theta.size(); ++i) {
    double start_x = x[i];
    double start_y = y[i];

    auto path_point_seg = Interpolate(
        start_x, start_y, start_s, theta[i], kappa[i], dkappa[i], theta[i + 1],
        kappa[i + 1], dkappa[i + 1], s[i], resolution);

    smoothed_point2d.insert(smoothed_point2d.end(), path_point_seg.begin(),
                            path_point_seg.end());

    start_s = smoothed_point2d.back().s();
  }
  return smoothed_point2d;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const double start_x, const double start_y, const double start_s,
    const double theta0, const double kappa0, const double dkappa0,
    const double theta1, const double kappa1, const double dkappa1,
    const double delta_s, const double resolution) const {
  std::vector<common::PathPoint> path_points;

  QuinticSpiralPath spiral_curve(theta0, kappa0, dkappa0, theta1, kappa1,
                                 dkappa1, delta_s);
  std::size_t num_of_points = std::ceil(delta_s / resolution) + 1;
  for (std::size_t i = 1; i <= num_of_points; ++i) {
    const double inter_s = delta_s / num_of_points * i;
    const double dx = spiral_curve.ComputeCartesianDeviationX<10>(inter_s);
    const double dy = spiral_curve.ComputeCartesianDeviationY<10>(inter_s);

    const double theta =
        common::math::NormalizeAngle(spiral_curve.Evaluate(0, inter_s));
    const double kappa = spiral_curve.Evaluate(1, inter_s);
    const double dkappa = spiral_curve.Evaluate(2, inter_s);

    auto path_point = to_path_point(start_x + dx, start_y + dy,
                                    start_s + inter_s, theta, kappa, dkappa);
    path_points.push_back(std::move(path_point));
  }
  return path_points;
}

common::PathPoint SpiralReferenceLineSmoother::to_path_point(
    const double x, const double y, const double s, const double theta,
    const double kappa, const double dkappa) const {
  common::PathPoint point;
  point.set_x(x);
  point.set_y(y);
  point.set_s(s);
  point.set_theta(theta);
  point.set_kappa(kappa);
  point.set_dkappa(dkappa);
  return point;
}

void SpiralReferenceLineSmoother::SetAnchorPoints(
    const std::vector<AnchorPoint>& anchor_points) {
  anchor_points_ = std::move(anchor_points);

  CHECK_GT(anchor_points_.size(), 1);
  zero_x_ = anchor_points_.front().path_point.x();
  zero_y_ = anchor_points_.front().path_point.y();

  std::for_each(anchor_points_.begin(), anchor_points_.end(),
      [this](AnchorPoint& p) {
        auto curr_x = p.path_point.x();
        auto curr_y = p.path_point.y();
        p.path_point.set_x(curr_x - zero_x_);
        p.path_point.set_y(curr_y - zero_y_);});
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: further tuned parameters for spiral reference line smoother<commit_after>/******************************************************************************
 * Copyright 2017 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.
 *****************************************************************************/

/*
 * spiral_reference_line_smoother.cc
 */

#include "modules/planning/reference_line/spiral_reference_line_smoother.h"

#include <algorithm>
#include <iomanip>
#include <utility>

#include "IpIpoptApplication.hpp"
#include "IpSolveStatistics.hpp"

#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quintic_spiral_path.h"
#include "modules/planning/reference_line/spiral_problem_interface.h"

namespace apollo {
namespace planning {

using apollo::common::time::Clock;

SpiralReferenceLineSmoother::SpiralReferenceLineSmoother(
    const double max_point_deviation)
    : default_max_point_deviation_(max_point_deviation) {
  CHECK(max_point_deviation >= 0.0);
}

bool SpiralReferenceLineSmoother::Smooth(
    const ReferenceLine& raw_reference_line,
    ReferenceLine* const smoothed_reference_line) {
  const double start_timestamp = Clock::NowInSeconds();
  std::vector<double> opt_x;
  std::vector<double> opt_y;
  std::vector<double> opt_theta;
  std::vector<double> opt_kappa;
  std::vector<double> opt_dkappa;
  std::vector<double> opt_s;

  if (anchor_points_.empty()) {
    const double piecewise_length = FLAGS_spiral_smoother_piecewise_length;
    const double length = raw_reference_line.Length();
    ADEBUG << "Length = " << length;
    uint32_t num_of_pieces =
        std::max(1u, static_cast<uint32_t>(length / piecewise_length));

    const double delta_s = length / num_of_pieces;
    double s = 0.0;

    std::vector<Eigen::Vector2d> raw_point2d;
    for (std::uint32_t i = 0; i <= num_of_pieces;
         ++i, s = std::fmin(s + delta_s, length)) {
      ReferencePoint rlp = raw_reference_line.GetReferencePoint(s);
      raw_point2d.emplace_back(rlp.x(), rlp.y());
    }

    Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
           &opt_y);
  } else {
    std::size_t start_index = 0;
    for (const auto& anchor_point : anchor_points_) {
      if (anchor_point.enforced) {
        start_index++;
      } else {
        break;
      }
    }

    std::vector<Eigen::Vector2d> raw_point2d;
    if (start_index == 0) {
      for (const auto& anchor_point : anchor_points_) {
        raw_point2d.emplace_back(anchor_point.path_point.x(),
                                 anchor_point.path_point.y());
      }
    } else {
      std::vector<double> overhead_s;
      for (std::size_t i = 0; i + 1 < start_index; ++i) {
        const auto& p0 = anchor_points_[i];
        const auto& p1 = anchor_points_[i + 1];
        overhead_s.push_back(p1.path_point.s() - p0.path_point.s());
      }

      std::vector<double> overhead_theta;
      std::vector<double> overhead_kappa;
      std::vector<double> overhead_dkappa;
      std::vector<double> overhead_x;
      std::vector<double> overhead_y;
      for (std::size_t i = 0; i < anchor_points_.size(); ++i) {
        const auto& p = anchor_points_[i];
        if (i + 1 < start_index) {
          overhead_theta.push_back(p.path_point.theta());
          overhead_kappa.push_back(p.path_point.kappa());
          overhead_dkappa.push_back(p.path_point.dkappa());
          overhead_x.push_back(p.path_point.x());
          overhead_y.push_back(p.path_point.y());
        } else {
          raw_point2d.emplace_back(p.path_point.x(), p.path_point.y());
        }
      }

      fixed_start_point_ = true;
      fixed_start_x_ = anchor_points_[start_index - 1].path_point.x();
      fixed_start_y_ = anchor_points_[start_index - 1].path_point.y();
      fixed_start_theta_ = common::math::NormalizeAngle(
          anchor_points_[start_index - 1].path_point.theta());
      fixed_start_kappa_ = anchor_points_[start_index - 1].path_point.kappa();
      fixed_start_dkappa_ = anchor_points_[start_index - 1].path_point.dkappa();

      Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
             &opt_y);

      opt_theta.insert(opt_theta.begin(), overhead_theta.begin(),
                       overhead_theta.end());
      opt_kappa.insert(opt_kappa.begin(), overhead_kappa.begin(),
                       overhead_kappa.end());
      opt_dkappa.insert(opt_dkappa.begin(), overhead_dkappa.begin(),
                        overhead_dkappa.end());
      opt_s.insert(opt_s.begin(), overhead_s.begin(), overhead_s.end());
      opt_x.insert(opt_x.begin(), overhead_x.begin(), overhead_x.end());
      opt_y.insert(opt_y.begin(), overhead_y.begin(), overhead_y.end());

      std::for_each(opt_x.begin(), opt_x.end(), [this](double& x){
        x += zero_x_;
      });

      std::for_each(opt_y.begin(), opt_y.end(), [this](double& y){
        y += zero_y_;
      });
    }
  }

  std::vector<common::PathPoint> smoothed_point2d =
      Interpolate(opt_theta, opt_kappa, opt_dkappa, opt_s, opt_x, opt_y,
                  FLAGS_spiral_reference_line_resolution);

  std::vector<ReferencePoint> ref_points;
  for (const auto& p : smoothed_point2d) {
    const double heading = p.theta();
    const double kappa = p.kappa();
    const double dkappa = p.dkappa();

    common::SLPoint ref_sl_point;
    if (!raw_reference_line.XYToSL({p.x(), p.y()}, &ref_sl_point)) {
      return false;
    }
    if (ref_sl_point.s() < 0 ||
        ref_sl_point.s() > raw_reference_line.Length()) {
      continue;
    }

    ReferencePoint rlp = raw_reference_line.GetReferencePoint(ref_sl_point.s());
    ref_points.emplace_back(
        ReferencePoint(hdmap::MapPathPoint(common::math::Vec2d(p.x(), p.y()),
                                           heading, rlp.lane_waypoints()),
                       kappa, dkappa, 0.0, 0.0));
  }

  ReferencePoint::RemoveDuplicates(&ref_points);
  if (ref_points.size() < 2) {
    AERROR << "Fail to generate smoothed reference line.";
    return false;
  }
  *smoothed_reference_line = ReferenceLine(ref_points);
  const double end_timestamp = Clock::NowInSeconds();
  ADEBUG << "Spiral reference line smoother time: "
         << (end_timestamp - start_timestamp) * 1000 << " ms.";
  return true;
}

bool SpiralReferenceLineSmoother::Smooth(std::vector<Eigen::Vector2d> point2d,
                                         std::vector<double>* ptr_theta,
                                         std::vector<double>* ptr_kappa,
                                         std::vector<double>* ptr_dkappa,
                                         std::vector<double>* ptr_s,
                                         std::vector<double>* ptr_x,
                                         std::vector<double>* ptr_y) const {
  CHECK_GT(point2d.size(), 1);

  SpiralProblemInterface* ptop = new SpiralProblemInterface(point2d);
  ptop->set_default_max_point_deviation(default_max_point_deviation_);
  if (fixed_start_point_) {
    ptop->set_start_point(fixed_start_x_, fixed_start_y_, fixed_start_theta_,
                          fixed_start_kappa_, fixed_start_dkappa_);
  }

  Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;

  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();

  app->Options()->SetStringValue("hessian_approximation", "limited-memory");
  //  app->Options()->SetStringValue("derivative_test", "first-order");
  //  app->Options()->SetNumericValue("derivative_test_perturbation", 1.0e-7);
  //  app->Options()->SetStringValue("derivative_test", "second-order");
  app->Options()->SetIntegerValue("print_level", 0);
  int num_iterations = FLAGS_spiral_smoother_num_iteration;
  app->Options()->SetIntegerValue("max_iter", num_iterations);
  app->Options()->SetIntegerValue("acceptable_iter", 5);
  app->Options()->SetNumericValue("tol", 1.0e-4);
  app->Options()->SetNumericValue("acceptable_tol", 1.0e-5);

  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    ADEBUG << "*** Error during initialization!";
    return static_cast<int>(status);
  }

  status = app->OptimizeTNLP(problem);

  if (status == Ipopt::Solve_Succeeded ||
      status == Ipopt::Solved_To_Acceptable_Level) {
    // Retrieve some statistics about the solve
    Ipopt::Index iter_count = app->Statistics()->IterationCount();
    ADEBUG << "*** The problem solved in " << iter_count << " iterations!";

    Ipopt::Number final_obj = app->Statistics()->FinalObjective();
    ADEBUG << "*** The final value of the objective function is " << final_obj
           << '.';
  } else {
    ADEBUG << "Return status: " << int(status);
  }

  ptop->get_optimization_results(ptr_theta, ptr_kappa, ptr_dkappa, ptr_s, ptr_x,
                                 ptr_y);

  return status == Ipopt::Solve_Succeeded ||
         status == Ipopt::Solved_To_Acceptable_Level;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const std::vector<double>& theta, const std::vector<double>& kappa,
    const std::vector<double>& dkappa, const std::vector<double>& s,
    const std::vector<double>& x, const std::vector<double>& y,
    const double resolution) const {
  std::vector<common::PathPoint> smoothed_point2d;
  double start_s = 0.0;
  common::PathPoint first_point =
      to_path_point(x.front(), y.front(), start_s, theta.front(), kappa.front(),
                    dkappa.front());
  smoothed_point2d.push_back(first_point);

  for (std::size_t i = 0; i + 1 < theta.size(); ++i) {
    double start_x = x[i];
    double start_y = y[i];

    auto path_point_seg = Interpolate(
        start_x, start_y, start_s, theta[i], kappa[i], dkappa[i], theta[i + 1],
        kappa[i + 1], dkappa[i + 1], s[i], resolution);

    smoothed_point2d.insert(smoothed_point2d.end(), path_point_seg.begin(),
                            path_point_seg.end());

    start_s = smoothed_point2d.back().s();
  }
  return smoothed_point2d;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const double start_x, const double start_y, const double start_s,
    const double theta0, const double kappa0, const double dkappa0,
    const double theta1, const double kappa1, const double dkappa1,
    const double delta_s, const double resolution) const {
  std::vector<common::PathPoint> path_points;

  QuinticSpiralPath spiral_curve(theta0, kappa0, dkappa0, theta1, kappa1,
                                 dkappa1, delta_s);
  std::size_t num_of_points = std::ceil(delta_s / resolution) + 1;
  for (std::size_t i = 1; i <= num_of_points; ++i) {
    const double inter_s = delta_s / num_of_points * i;
    const double dx = spiral_curve.ComputeCartesianDeviationX<10>(inter_s);
    const double dy = spiral_curve.ComputeCartesianDeviationY<10>(inter_s);

    const double theta =
        common::math::NormalizeAngle(spiral_curve.Evaluate(0, inter_s));
    const double kappa = spiral_curve.Evaluate(1, inter_s);
    const double dkappa = spiral_curve.Evaluate(2, inter_s);

    auto path_point = to_path_point(start_x + dx, start_y + dy,
                                    start_s + inter_s, theta, kappa, dkappa);
    path_points.push_back(std::move(path_point));
  }
  return path_points;
}

common::PathPoint SpiralReferenceLineSmoother::to_path_point(
    const double x, const double y, const double s, const double theta,
    const double kappa, const double dkappa) const {
  common::PathPoint point;
  point.set_x(x);
  point.set_y(y);
  point.set_s(s);
  point.set_theta(theta);
  point.set_kappa(kappa);
  point.set_dkappa(dkappa);
  return point;
}

void SpiralReferenceLineSmoother::SetAnchorPoints(
    const std::vector<AnchorPoint>& anchor_points) {
  anchor_points_ = std::move(anchor_points);

  CHECK_GT(anchor_points_.size(), 1);
  zero_x_ = anchor_points_.front().path_point.x();
  zero_y_ = anchor_points_.front().path_point.y();

  std::for_each(anchor_points_.begin(), anchor_points_.end(),
      [this](AnchorPoint& p) {
        auto curr_x = p.path_point.x();
        auto curr_y = p.path_point.y();
        p.path_point.set_x(curr_x - zero_x_);
        p.path_point.set_y(curr_y - zero_y_);});
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>//===- MarkLive.cpp -------------------------------------------------------===//
//
//                             The LLVM Linker
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file implements --gc-sections, which is a feature to remove unused
// sections from output. Unused sections are sections that are not reachable
// from known GC-root symbols or sections. Naturally the feature is
// implemented as a mark-sweep garbage collector.
//
// Here's how it works. Each InputSectionBase has a "Live" bit. The bit is off
// by default. Starting with GC-root symbols or sections, markLive function
// defined in this file visits all reachable sections to set their Live
// bits. Writer will then ignore sections whose Live bits are off, so that
// such sections are not included into output.
//
//===----------------------------------------------------------------------===//

#include "MarkLive.h"
#include "InputSection.h"
#include "LinkerScript.h"
#include "OutputSections.h"
#include "SymbolTable.h"
#include "Symbols.h"
#include "Target.h"
#include "lld/Common/Memory.h"
#include "lld/Common/Strings.h"
#include "llvm/ADT/STLExtras.h"
#include "llvm/Object/ELF.h"
#include <functional>
#include <vector>

using namespace llvm;
using namespace llvm::ELF;
using namespace llvm::object;
using namespace llvm::support::endian;

using namespace lld;
using namespace lld::elf;

template <class ELFT>
static typename ELFT::uint getAddend(InputSectionBase &Sec,
                                     const typename ELFT::Rel &Rel) {
  return Target->getImplicitAddend(Sec.data().begin() + Rel.r_offset,
                                   Rel.getType(Config->IsMips64EL));
}

template <class ELFT>
static typename ELFT::uint getAddend(InputSectionBase &Sec,
                                     const typename ELFT::Rela &Rel) {
  return Rel.r_addend;
}

// There are normally few input sections whose names are valid C
// identifiers, so we just store a std::vector instead of a multimap.
static DenseMap<StringRef, std::vector<InputSectionBase *>> CNamedSections;

template <class ELFT, class RelT>
static void
resolveReloc(InputSectionBase &Sec, RelT &Rel,
             llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  Symbol &B = Sec.getFile<ELFT>()->getRelocTargetSym(Rel);

  // If a symbol is referenced in a live section, it is used.
  B.Used = true;
  if (auto *SS = dyn_cast<SharedSymbol>(&B))
    if (!SS->isWeak())
      SS->getFile<ELFT>().IsNeeded = true;

  if (auto *D = dyn_cast<Defined>(&B)) {
    auto *RelSec = dyn_cast_or_null<InputSectionBase>(D->Section);
    if (!RelSec)
      return;
    uint64_t Offset = D->Value;
    if (D->isSection())
      Offset += getAddend<ELFT>(Sec, Rel);
    Fn(RelSec, Offset);
    return;
  }

  if (!B.isDefined())
    for (InputSectionBase *Sec : CNamedSections.lookup(B.getName()))
      Fn(Sec, 0);
}

// Calls Fn for each section that Sec refers to via relocations.
template <class ELFT>
static void
forEachSuccessor(InputSection &Sec,
                 llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  if (Sec.AreRelocsRela) {
    for (const typename ELFT::Rela &Rel : Sec.template relas<ELFT>())
      resolveReloc<ELFT>(Sec, Rel, Fn);
  } else {
    for (const typename ELFT::Rel &Rel : Sec.template rels<ELFT>())
      resolveReloc<ELFT>(Sec, Rel, Fn);
  }

  for (InputSectionBase *IS : Sec.DependentSections)
    Fn(IS, 0);
}

// The .eh_frame section is an unfortunate special case.
// The section is divided in CIEs and FDEs and the relocations it can have are
// * CIEs can refer to a personality function.
// * FDEs can refer to a LSDA
// * FDEs refer to the function they contain information about
// The last kind of relocation cannot keep the referred section alive, or they
// would keep everything alive in a common object file. In fact, each FDE is
// alive if the section it refers to is alive.
// To keep things simple, in here we just ignore the last relocation kind. The
// other two keep the referred section alive.
//
// A possible improvement would be to fully process .eh_frame in the middle of
// the gc pass. With that we would be able to also gc some sections holding
// LSDAs and personality functions if we found that they were unused.
template <class ELFT, class RelTy>
static void
scanEhFrameSection(EhInputSection &EH, ArrayRef<RelTy> Rels,
                   llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  const endianness E = ELFT::TargetEndianness;

  for (unsigned I = 0, N = EH.Pieces.size(); I < N; ++I) {
    EhSectionPiece &Piece = EH.Pieces[I];
    unsigned FirstRelI = Piece.FirstRelocation;
    if (FirstRelI == (unsigned)-1)
      continue;
    if (read32<E>(Piece.data().data() + 4) == 0) {
      // This is a CIE, we only need to worry about the first relocation. It is
      // known to point to the personality function.
      resolveReloc<ELFT>(EH, Rels[FirstRelI], Fn);
      continue;
    }
    // This is a FDE. The relocations point to the described function or to
    // a LSDA. We only need to keep the LSDA alive, so ignore anything that
    // points to executable sections.
    typename ELFT::uint PieceEnd = Piece.InputOff + Piece.Size;
    for (unsigned I2 = FirstRelI, N2 = Rels.size(); I2 < N2; ++I2) {
      const RelTy &Rel = Rels[I2];
      if (Rel.r_offset >= PieceEnd)
        break;
      resolveReloc<ELFT>(EH, Rels[I2],
                         [&](InputSectionBase *Sec, uint64_t Offset) {
                           if (Sec && Sec != &InputSection::Discarded &&
                               !(Sec->Flags & SHF_EXECINSTR))
                             Fn(Sec, 0);
                         });
    }
  }
}

template <class ELFT>
static void
scanEhFrameSection(EhInputSection &EH,
                   llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  if (!EH.NumRelocations)
    return;

  if (EH.AreRelocsRela)
    scanEhFrameSection<ELFT>(EH, EH.template relas<ELFT>(), Fn);
  else
    scanEhFrameSection<ELFT>(EH, EH.template rels<ELFT>(), Fn);
}

// Some sections are used directly by the loader, so they should never be
// garbage-collected. This function returns true if a given section is such
// section.
template <class ELFT> static bool isReserved(InputSectionBase *Sec) {
  switch (Sec->Type) {
  case SHT_FINI_ARRAY:
  case SHT_INIT_ARRAY:
  case SHT_NOTE:
  case SHT_PREINIT_ARRAY:
    return true;
  default:
    StringRef S = Sec->Name;
    return S.startswith(".ctors") || S.startswith(".dtors") ||
           S.startswith(".init") || S.startswith(".fini") ||
           S.startswith(".jcr");
  }
}

// This is the main function of the garbage collector.
// Starting from GC-root sections, this function visits all reachable
// sections to set their "Live" bits.
template <class ELFT> static void doGcSections() {
  SmallVector<InputSection *, 256> Q;
  CNamedSections.clear();

  auto Enqueue = [&](InputSectionBase *Sec, uint64_t Offset) {
    // Skip over discarded sections. This in theory shouldn't happen, because
    // the ELF spec doesn't allow a relocation to point to a deduplicated
    // COMDAT section directly. Unfortunately this happens in practice (e.g.
    // .eh_frame) so we need to add a check.
    if (Sec == &InputSection::Discarded)
      return;

    // Usually, a whole section is marked as live or dead, but in mergeable
    // (splittable) sections, each piece of data has independent liveness bit.
    // So we explicitly tell it which offset is in use.
    if (auto *MS = dyn_cast<MergeInputSection>(Sec))
      MS->getSectionPiece(Offset)->Live = true;

    if (Sec->Live)
      return;
    Sec->Live = true;

    // Add input section to the queue.
    if (InputSection *S = dyn_cast<InputSection>(Sec))
      Q.push_back(S);
  };

  auto MarkSymbol = [&](Symbol *Sym) {
    if (auto *D = dyn_cast_or_null<Defined>(Sym))
      if (auto *IS = dyn_cast_or_null<InputSectionBase>(D->Section))
        Enqueue(IS, D->Value);
  };

  // Add GC root symbols.
  MarkSymbol(Symtab->find(Config->Entry));
  MarkSymbol(Symtab->find(Config->Init));
  MarkSymbol(Symtab->find(Config->Fini));
  for (StringRef S : Config->Undefined)
    MarkSymbol(Symtab->find(S));
  for (StringRef S : Script->ReferencedSymbols)
    MarkSymbol(Symtab->find(S));

  // Preserve externally-visible symbols if the symbols defined by this
  // file can interrupt other ELF file's symbols at runtime.
  for (Symbol *S : Symtab->getSymbols())
    if (S->includeInDynsym())
      MarkSymbol(S);

  // Preserve special sections and those which are specified in linker
  // script KEEP command.
  for (InputSectionBase *Sec : InputSections) {
    // Mark .eh_frame sections as live because there are usually no relocations
    // that point to .eh_frames. Otherwise, the garbage collector would drop
    // all of them. We also want to preserve personality routines and LSDA
    // referenced by .eh_frame sections, so we scan them for that here.
    if (auto *EH = dyn_cast<EhInputSection>(Sec)) {
      EH->Live = true;
      scanEhFrameSection<ELFT>(*EH, Enqueue);
    }

    if (Sec->Flags & SHF_LINK_ORDER)
      continue;
    if (isReserved<ELFT>(Sec) || Script->shouldKeep(Sec))
      Enqueue(Sec, 0);
    else if (isValidCIdentifier(Sec->Name)) {
      CNamedSections[Saver.save("__start_" + Sec->Name)].push_back(Sec);
      CNamedSections[Saver.save("__stop_" + Sec->Name)].push_back(Sec);
    }
  }

  // Mark all reachable sections.
  while (!Q.empty())
    forEachSuccessor<ELFT>(*Q.pop_back_val(), Enqueue);
}

// Before calling this function, Live bits are off for all
// input sections. This function make some or all of them on
// so that they are emitted to the output file.
template <class ELFT> void elf::markLive() {
  if (!Config->GcSections) {
    // If -gc-sections is missing, no sections are removed.
    for (InputSectionBase *Sec : InputSections)
      Sec->Live = true;

    // If a DSO defines a symbol referenced in a regular object, it is needed.
    for (Symbol *Sym : Symtab->getSymbols())
      if (auto *S = dyn_cast<SharedSymbol>(Sym))
        if (S->IsUsedInRegularObj && !S->isWeak())
          S->getFile<ELFT>().IsNeeded = true;
    return;
  }

  // The -gc-sections option works only for SHF_ALLOC sections
  // (sections that are memory-mapped at runtime). So we can
  // unconditionally make non-SHF_ALLOC sections alive except
  // SHF_LINK_ORDER and SHT_REL/SHT_RELA sections.
  //
  // Usually, SHF_ALLOC sections are not removed even if they are
  // unreachable through relocations because reachability is not
  // a good signal whether they are garbage or not (e.g. there is
  // usually no section referring to a .comment section, but we
  // want to keep it.).
  //
  // Note on SHF_LINK_ORDER: Such sections contain metadata and they
  // have a reverse dependency on the InputSection they are linked with.
  // We are able to garbage collect them.
  //
  // Note on SHF_REL{,A}: Such sections reach here only when -r
  // or -emit-reloc were given. And they are subject of garbage
  // collection because, if we remove a text section, we also
  // remove its relocation section.
  for (InputSectionBase *Sec : InputSections) {
    bool IsAlloc = (Sec->Flags & SHF_ALLOC);
    bool IsLinkOrder = (Sec->Flags & SHF_LINK_ORDER);
    bool IsRel = (Sec->Type == SHT_REL || Sec->Type == SHT_RELA);
    if (!IsAlloc && !IsLinkOrder && !IsRel)
      Sec->Live = true;
  }

  // Follow the graph to mark all live sections.
  doGcSections<ELFT>();

  // Report garbage-collected sections.
  if (Config->PrintGcSections)
    for (InputSectionBase *Sec : InputSections)
      if (!Sec->Live)
        message("removing unused section " + toString(Sec));
}

template void elf::markLive<ELF32LE>();
template void elf::markLive<ELF32BE>();
template void elf::markLive<ELF64LE>();
template void elf::markLive<ELF64BE>();
<commit_msg>Fix formatting. NFC.<commit_after>//===- MarkLive.cpp -------------------------------------------------------===//
//
//                             The LLVM Linker
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file implements --gc-sections, which is a feature to remove unused
// sections from output. Unused sections are sections that are not reachable
// from known GC-root symbols or sections. Naturally the feature is
// implemented as a mark-sweep garbage collector.
//
// Here's how it works. Each InputSectionBase has a "Live" bit. The bit is off
// by default. Starting with GC-root symbols or sections, markLive function
// defined in this file visits all reachable sections to set their Live
// bits. Writer will then ignore sections whose Live bits are off, so that
// such sections are not included into output.
//
//===----------------------------------------------------------------------===//

#include "MarkLive.h"
#include "InputSection.h"
#include "LinkerScript.h"
#include "OutputSections.h"
#include "SymbolTable.h"
#include "Symbols.h"
#include "Target.h"
#include "lld/Common/Memory.h"
#include "lld/Common/Strings.h"
#include "llvm/ADT/STLExtras.h"
#include "llvm/Object/ELF.h"
#include <functional>
#include <vector>

using namespace llvm;
using namespace llvm::ELF;
using namespace llvm::object;
using namespace llvm::support::endian;

using namespace lld;
using namespace lld::elf;

template <class ELFT>
static typename ELFT::uint getAddend(InputSectionBase &Sec,
                                     const typename ELFT::Rel &Rel) {
  return Target->getImplicitAddend(Sec.data().begin() + Rel.r_offset,
                                   Rel.getType(Config->IsMips64EL));
}

template <class ELFT>
static typename ELFT::uint getAddend(InputSectionBase &Sec,
                                     const typename ELFT::Rela &Rel) {
  return Rel.r_addend;
}

// There are normally few input sections whose names are valid C
// identifiers, so we just store a std::vector instead of a multimap.
static DenseMap<StringRef, std::vector<InputSectionBase *>> CNamedSections;

template <class ELFT, class RelT>
static void
resolveReloc(InputSectionBase &Sec, RelT &Rel,
             llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  Symbol &B = Sec.getFile<ELFT>()->getRelocTargetSym(Rel);

  // If a symbol is referenced in a live section, it is used.
  B.Used = true;
  if (auto *SS = dyn_cast<SharedSymbol>(&B))
    if (!SS->isWeak())
      SS->getFile<ELFT>().IsNeeded = true;

  if (auto *D = dyn_cast<Defined>(&B)) {
    auto *RelSec = dyn_cast_or_null<InputSectionBase>(D->Section);
    if (!RelSec)
      return;
    uint64_t Offset = D->Value;
    if (D->isSection())
      Offset += getAddend<ELFT>(Sec, Rel);
    Fn(RelSec, Offset);
    return;
  }

  if (!B.isDefined())
    for (InputSectionBase *Sec : CNamedSections.lookup(B.getName()))
      Fn(Sec, 0);
}

// Calls Fn for each section that Sec refers to via relocations.
template <class ELFT>
static void
forEachSuccessor(InputSection &Sec,
                 llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  if (Sec.AreRelocsRela) {
    for (const typename ELFT::Rela &Rel : Sec.template relas<ELFT>())
      resolveReloc<ELFT>(Sec, Rel, Fn);
  } else {
    for (const typename ELFT::Rel &Rel : Sec.template rels<ELFT>())
      resolveReloc<ELFT>(Sec, Rel, Fn);
  }

  for (InputSectionBase *IS : Sec.DependentSections)
    Fn(IS, 0);
}

// The .eh_frame section is an unfortunate special case.
// The section is divided in CIEs and FDEs and the relocations it can have are
// * CIEs can refer to a personality function.
// * FDEs can refer to a LSDA
// * FDEs refer to the function they contain information about
// The last kind of relocation cannot keep the referred section alive, or they
// would keep everything alive in a common object file. In fact, each FDE is
// alive if the section it refers to is alive.
// To keep things simple, in here we just ignore the last relocation kind. The
// other two keep the referred section alive.
//
// A possible improvement would be to fully process .eh_frame in the middle of
// the gc pass. With that we would be able to also gc some sections holding
// LSDAs and personality functions if we found that they were unused.
template <class ELFT, class RelTy>
static void
scanEhFrameSection(EhInputSection &EH, ArrayRef<RelTy> Rels,
                   llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  const endianness E = ELFT::TargetEndianness;

  for (unsigned I = 0, N = EH.Pieces.size(); I < N; ++I) {
    EhSectionPiece &Piece = EH.Pieces[I];
    unsigned FirstRelI = Piece.FirstRelocation;
    if (FirstRelI == (unsigned)-1)
      continue;
    if (read32<E>(Piece.data().data() + 4) == 0) {
      // This is a CIE, we only need to worry about the first relocation. It is
      // known to point to the personality function.
      resolveReloc<ELFT>(EH, Rels[FirstRelI], Fn);
      continue;
    }
    // This is a FDE. The relocations point to the described function or to
    // a LSDA. We only need to keep the LSDA alive, so ignore anything that
    // points to executable sections.
    typename ELFT::uint PieceEnd = Piece.InputOff + Piece.Size;
    for (unsigned I2 = FirstRelI, N2 = Rels.size(); I2 < N2; ++I2) {
      const RelTy &Rel = Rels[I2];
      if (Rel.r_offset >= PieceEnd)
        break;
      resolveReloc<ELFT>(EH, Rels[I2],
                         [&](InputSectionBase *Sec, uint64_t Offset) {
                           if (Sec && Sec != &InputSection::Discarded &&
                               !(Sec->Flags & SHF_EXECINSTR))
                             Fn(Sec, 0);
                         });
    }
  }
}

template <class ELFT>
static void
scanEhFrameSection(EhInputSection &EH,
                   llvm::function_ref<void(InputSectionBase *, uint64_t)> Fn) {
  if (!EH.NumRelocations)
    return;

  if (EH.AreRelocsRela)
    scanEhFrameSection<ELFT>(EH, EH.template relas<ELFT>(), Fn);
  else
    scanEhFrameSection<ELFT>(EH, EH.template rels<ELFT>(), Fn);
}

// Some sections are used directly by the loader, so they should never be
// garbage-collected. This function returns true if a given section is such
// section.
template <class ELFT> static bool isReserved(InputSectionBase *Sec) {
  switch (Sec->Type) {
  case SHT_FINI_ARRAY:
  case SHT_INIT_ARRAY:
  case SHT_NOTE:
  case SHT_PREINIT_ARRAY:
    return true;
  default:
    StringRef S = Sec->Name;
    return S.startswith(".ctors") || S.startswith(".dtors") ||
           S.startswith(".init") || S.startswith(".fini") ||
           S.startswith(".jcr");
  }
}

// This is the main function of the garbage collector.
// Starting from GC-root sections, this function visits all reachable
// sections to set their "Live" bits.
template <class ELFT> static void doGcSections() {
  SmallVector<InputSection *, 256> Q;
  CNamedSections.clear();

  auto Enqueue = [&](InputSectionBase *Sec, uint64_t Offset) {
    // Skip over discarded sections. This in theory shouldn't happen, because
    // the ELF spec doesn't allow a relocation to point to a deduplicated
    // COMDAT section directly. Unfortunately this happens in practice (e.g.
    // .eh_frame) so we need to add a check.
    if (Sec == &InputSection::Discarded)
      return;

    // Usually, a whole section is marked as live or dead, but in mergeable
    // (splittable) sections, each piece of data has independent liveness bit.
    // So we explicitly tell it which offset is in use.
    if (auto *MS = dyn_cast<MergeInputSection>(Sec))
      MS->getSectionPiece(Offset)->Live = true;

    if (Sec->Live)
      return;
    Sec->Live = true;

    // Add input section to the queue.
    if (InputSection *S = dyn_cast<InputSection>(Sec))
      Q.push_back(S);
  };

  auto MarkSymbol = [&](Symbol *Sym) {
    if (auto *D = dyn_cast_or_null<Defined>(Sym))
      if (auto *IS = dyn_cast_or_null<InputSectionBase>(D->Section))
        Enqueue(IS, D->Value);
  };

  // Add GC root symbols.
  MarkSymbol(Symtab->find(Config->Entry));
  MarkSymbol(Symtab->find(Config->Init));
  MarkSymbol(Symtab->find(Config->Fini));
  for (StringRef S : Config->Undefined)
    MarkSymbol(Symtab->find(S));
  for (StringRef S : Script->ReferencedSymbols)
    MarkSymbol(Symtab->find(S));

  // Preserve externally-visible symbols if the symbols defined by this
  // file can interrupt other ELF file's symbols at runtime.
  for (Symbol *S : Symtab->getSymbols())
    if (S->includeInDynsym())
      MarkSymbol(S);

  // Preserve special sections and those which are specified in linker
  // script KEEP command.
  for (InputSectionBase *Sec : InputSections) {
    // Mark .eh_frame sections as live because there are usually no relocations
    // that point to .eh_frames. Otherwise, the garbage collector would drop
    // all of them. We also want to preserve personality routines and LSDA
    // referenced by .eh_frame sections, so we scan them for that here.
    if (auto *EH = dyn_cast<EhInputSection>(Sec)) {
      EH->Live = true;
      scanEhFrameSection<ELFT>(*EH, Enqueue);
    }

    if (Sec->Flags & SHF_LINK_ORDER)
      continue;

    if (isReserved<ELFT>(Sec) || Script->shouldKeep(Sec)) {
      Enqueue(Sec, 0);
    } else if (isValidCIdentifier(Sec->Name)) {
      CNamedSections[Saver.save("__start_" + Sec->Name)].push_back(Sec);
      CNamedSections[Saver.save("__stop_" + Sec->Name)].push_back(Sec);
    }
  }

  // Mark all reachable sections.
  while (!Q.empty())
    forEachSuccessor<ELFT>(*Q.pop_back_val(), Enqueue);
}

// Before calling this function, Live bits are off for all
// input sections. This function make some or all of them on
// so that they are emitted to the output file.
template <class ELFT> void elf::markLive() {
  if (!Config->GcSections) {
    // If -gc-sections is missing, no sections are removed.
    for (InputSectionBase *Sec : InputSections)
      Sec->Live = true;

    // If a DSO defines a symbol referenced in a regular object, it is needed.
    for (Symbol *Sym : Symtab->getSymbols())
      if (auto *S = dyn_cast<SharedSymbol>(Sym))
        if (S->IsUsedInRegularObj && !S->isWeak())
          S->getFile<ELFT>().IsNeeded = true;
    return;
  }

  // The -gc-sections option works only for SHF_ALLOC sections
  // (sections that are memory-mapped at runtime). So we can
  // unconditionally make non-SHF_ALLOC sections alive except
  // SHF_LINK_ORDER and SHT_REL/SHT_RELA sections.
  //
  // Usually, SHF_ALLOC sections are not removed even if they are
  // unreachable through relocations because reachability is not
  // a good signal whether they are garbage or not (e.g. there is
  // usually no section referring to a .comment section, but we
  // want to keep it.).
  //
  // Note on SHF_LINK_ORDER: Such sections contain metadata and they
  // have a reverse dependency on the InputSection they are linked with.
  // We are able to garbage collect them.
  //
  // Note on SHF_REL{,A}: Such sections reach here only when -r
  // or -emit-reloc were given. And they are subject of garbage
  // collection because, if we remove a text section, we also
  // remove its relocation section.
  for (InputSectionBase *Sec : InputSections) {
    bool IsAlloc = (Sec->Flags & SHF_ALLOC);
    bool IsLinkOrder = (Sec->Flags & SHF_LINK_ORDER);
    bool IsRel = (Sec->Type == SHT_REL || Sec->Type == SHT_RELA);
    if (!IsAlloc && !IsLinkOrder && !IsRel)
      Sec->Live = true;
  }

  // Follow the graph to mark all live sections.
  doGcSections<ELFT>();

  // Report garbage-collected sections.
  if (Config->PrintGcSections)
    for (InputSectionBase *Sec : InputSections)
      if (!Sec->Live)
        message("removing unused section " + toString(Sec));
}

template void elf::markLive<ELF32LE>();
template void elf::markLive<ELF32BE>();
template void elf::markLive<ELF64LE>();
template void elf::markLive<ELF64BE>();
<|endoftext|>


<commit_before>#include "lib/config.h"

#if defined(HAVE_R) && !defined(HAVE_SWIG)

#include "interface/RInterface.h"
#include "interface/SGInterface.h"

#include "lib/ShogunException.h"
#include "lib/io.h"

extern "C" {
#include <R.h>
#include <Rinternals.h>
#include <Rdefines.h>
#include <R_ext/Rdynload.h>
}

extern CSGInterface* interface;

CRInterface::CRInterface(SEXP prhs) : CSGInterface()
{
	m_nlhs=0;
	m_nrhs=length(prhs)-1;
	if (m_nrhs<0)
		m_nrhs=0;
	m_lhs=R_NilValue;
	m_rhs=prhs;
}

CRInterface::~CRInterface()
{
}

/** get functions - to pass data from the target interface to shogun */
void CRInterface::parse_args(INT num_args, INT num_default_args)
{
}

/// get type of current argument (does not increment argument counter)
IFType CRInterface::get_argument_type()
{
	SEXP arg=CADR(m_rhs);

	switch (TYPEOF(arg))
	{
		case INTSXP:
			return DENSE_INT;
		case REALSXP:
			return DENSE_REAL;
		case STRSXP:
			return STRING_CHAR;
	};
	return UNDEFINED;
}

INT CRInterface::get_int()
{
	SEXP i=get_arg_increment();
	if (i == R_NilValue || TYPEOF(CAR(i)) != INTSXP || Rf_nrows(CAR(i))!=1 || Rf_ncols(CAR(i))!=1)
		SG_ERROR("Expected Scalar Integer as argument %d\n", m_rhs_counter);

	return INTEGER(CAR(i))[0];
}

DREAL CRInterface::get_real()
{
	SEXP f=get_arg_increment();
	if (f == R_NilValue || TYPEOF(CAR(f)) != REALSXP || Rf_nrows(CAR(f))!=1 || Rf_ncols(CAR(f))!=1)
		SG_ERROR("Expected Scalar Float as argument %d\n", m_rhs_counter);

	return REAL(CAR(f))[0];
}

bool CRInterface::get_bool()
{
	SEXP b=get_arg_increment();
	if (b == R_NilValue || TYPEOF(CAR(b)) != LGLSXP || Rf_nrows(CAR(b))!=1 || Rf_ncols(CAR(b))!=1)
		SG_ERROR("Expected Scalar Boolean as argument %d\n", m_rhs_counter);

	return INTEGER(CAR(b))[0] != 0;
}

CHAR* CRInterface::get_string(INT& len)
{
	SEXP s=get_arg_increment();
	if (s == R_NilValue || TYPEOF(CAR(s)) != STRSXP || length(CAR(s))!=1)
		SG_ERROR("Expected String as argument %d\n", m_rhs_counter);

	SEXPREC* rstr= STRING_ELT(CAR(s),0);
	const CHAR* str= CHAR(rstr);
	len=LENGTH(rstr);
	ASSERT(len>0);
	CHAR* res=new CHAR[len+1];
	memcpy(res, str, len*sizeof(CHAR));
	res[len]='\0';
	return res;
}

void CRInterface::get_byte_vector(BYTE*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_char_vector(CHAR*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_int_vector(INT*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_shortreal_vector(SHORTREAL*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_real_vector(DREAL*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_short_vector(SHORT*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_word_vector(WORD*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_byte_matrix(BYTE*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_char_matrix(CHAR*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_int_matrix(INT*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_shortreal_matrix(SHORTREAL*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_real_matrix(DREAL*& matrix, INT& num_feat, INT& num_vec)
{
	SEXP feat=CAR(get_arg_increment());
	if( TYPEOF(feat) != REALSXP && TYPEOF(feat) != INTSXP )
		SG_ERROR("Expected Double Matrix as argument %d\n", m_rhs_counter);

	num_vec = Rf_ncols(feat);
	num_feat = Rf_nrows(feat);
	matrix=new DREAL[num_vec*num_feat];
	ASSERT(matrix);

	for (INT i=0; i<num_vec; i++)
		for (INT j=0; j<num_feat; j++)
			matrix[i*num_feat+j]= (DREAL) REAL(feat)[i*num_feat+j];
}

void CRInterface::get_short_matrix(SHORT*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_word_matrix(WORD*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_real_sparsematrix(TSparse<DREAL>*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_byte_string_list(T_STRING<BYTE>*& strings, INT& num_str, INT& max_string_len)
{
}

void CRInterface::get_char_string_list(T_STRING<CHAR>*& strings, INT& num_str, INT& max_string_len)
{
	SEXP strs=get_arg_increment();
	//SG_PRINT("len(strs): %d\n", length(CAR(strs))); || length(CAR(strs))>=1
	if (strs == R_NilValue || TYPEOF(CAR(strs)) != STRSXP)
		SG_ERROR("Expected String List as argument %d\n", m_rhs_counter);

	num_str=length(CAR(strs));
	strings=new T_STRING<CHAR>[num_str];
	ASSERT(strings);

	for (int i=0; i<num_str; i++)
	{
		SEXPREC* s= STRING_ELT(strs,i);
		CHAR* c= (CHAR*) CHAR(s);
		int len=LENGTH(s);

		if (len && c)
		{
			CHAR* dst=new CHAR[len];
			strings[i].string=(CHAR*) memcpy(dst, c, len*sizeof(CHAR));
			strings[i].length=len;
		}
		else
		{
			SG_WARNING( "string with index %d has zero length\n", i+1);
			strings[i].string=0;
			strings[i].length=0;
		}
	}
}

void CRInterface::get_int_string_list(T_STRING<INT>*& strings, INT& num_str, INT& max_string_len)
{
}

void CRInterface::get_short_string_list(T_STRING<SHORT>*& strings, INT& num_str, INT& max_string_len)
{
}

void CRInterface::get_word_string_list(T_STRING<WORD>*& strings, INT& num_str, INT& max_string_len)
{
	SEXP strs=get_arg_increment();
	if (strs == R_NilValue || TYPEOF(CAR(strs)) != STRSXP || length(CAR(strs))>=1)
		SG_ERROR("Expected String List as argument %d\n", m_rhs_counter);

	num_str=length(CAR(strs));
	strings=new T_STRING<WORD>[num_str];
	ASSERT(strings);

	for (int i=0; i<num_str; i++)
	{
		SEXPREC* s= STRING_ELT(strs,i);
		CHAR* c= (CHAR*) CHAR(s);
		int len=LENGTH(s);

		if (len && c)
		{
			WORD* dst=new WORD[len];
			strings[i].string=(WORD*) memcpy(dst, c, len*sizeof(CHAR));
			strings[i].length=len;
		}
		else
		{
			SG_WARNING( "string with index %d has zero length\n", i+1);
			strings[i].string=0;
			strings[i].length=0;
		}
	}
}

/** set functions - to pass data from shogun to the target interface */
bool CRInterface::create_return_values(INT num)
{
	if (num<=0)
		return true;

	PROTECT(m_lhs=allocVector(VECSXP, num));
	m_nlhs=num;
	return length(m_lhs) == num;
}

void CRInterface::set_byte_vector(const BYTE* vec, INT len)
{
}

void CRInterface::set_char_vector(const CHAR* vec, INT len)
{
}

void CRInterface::set_int_vector(const INT* vec, INT len)
{
}

void CRInterface::set_shortreal_vector(const SHORTREAL* vec, INT len)
{
}

void CRInterface::set_real_vector(const DREAL* vec, INT len)
{
}

void CRInterface::set_short_vector(const SHORT* vec, INT len)
{
}

void CRInterface::set_word_vector(const WORD* vec, INT len)
{
}

void CRInterface::set_byte_matrix(const BYTE* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_char_matrix(const CHAR* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_int_matrix(const INT* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_shortreal_matrix(const SHORTREAL* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_real_matrix(const DREAL* matrix, INT num_feat, INT num_vec)
{
	SEXP feat=NULL;
	PROTECT( feat = allocMatrix(REALSXP, num_feat, num_vec) );

	for (INT i=0; i<num_vec; i++)
	{
		for (INT j=0; j<num_feat; j++)
			REAL(feat)[i*num_feat+j]=(double) matrix[i*num_feat+j];
	}

	UNPROTECT(1);
	set_arg_increment(feat);															\
}

void CRInterface::set_short_matrix(const SHORT* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_word_matrix(const WORD* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_real_sparsematrix(const TSparse<DREAL>* matrix, INT num_feat, INT num_vec, LONG nnz)
{
}

void CRInterface::set_byte_string_list(const T_STRING<BYTE>* strings, INT num_str)
{
}

void CRInterface::set_char_string_list(const T_STRING<CHAR>* strings, INT num_str)
{
}

void CRInterface::set_int_string_list(const T_STRING<INT>* strings, INT num_str)
{
}

void CRInterface::set_short_string_list(const T_STRING<SHORT>* strings, INT num_str)
{
}

void CRInterface::set_word_string_list(const T_STRING<WORD>* strings, INT num_str)
{
}

/* The main function of the shogun R interface. All commands from the R command line
 * to the shogun backend are passed using the syntax:
 * .External("sg", "func", ... )
 * where '...' is a number of arguments passed to the shogun function 'func'. */

extern "C" {
/* This method is called by R when the shogun module is loaded into R
 * via dyn.load('sg.so'). */

SEXP sg(SEXP args);

void R_init_sg(DllInfo *info) {

   /* There are four different external language call mechanisms available in R, namely:
    *    .C
    *    .Call
    *    .Fortran
    *    .External
    *
    * Currently shogun uses only the .External interface. */

   R_CMethodDef cMethods[] = { {NULL, NULL, 0} };
   R_FortranMethodDef fortranMethods[] = { {NULL, NULL, 0} };
   R_ExternalMethodDef externalMethods[] = { {"sg", (void*(*)()) &sg, 1}, {NULL, NULL, 0} };
   R_CallMethodDef callMethods[] = { {NULL, NULL, 0} };

   /* Register the routines saved in the callMethods structure so that they are available under R. */
   R_registerRoutines(info, cMethods, callMethods, (R_FortranMethodDef*) fortranMethods, (R_ExternalMethodDef*) externalMethods);

}

SEXP sg(SEXP args)
{
	/* The SEXP (Simple Expression) args is a list of arguments of the .External call.
	 * it consists of "sg", "func" and additional arguments.
	 * */

	delete interface;
	interface=new CRInterface(args);

	try
	{
		if (!interface->handle())
			SG_ERROR("interface currently does not handle this command\n");
	}
	catch (ShogunException e)
	{
		return R_NilValue;
	}

	return ((CRInterface*) interface)->get_return_values();
}

/* This method is called form within R when the current module is unregistered.
 * Note that R does not allow unregistering of single symbols. */

void R_unload_sg(DllInfo *info) { }

} // extern "C"

#endif // HAVE_R && !HAVE_SWIG
<commit_msg>completed R interface - not many datatypes are supported in R :(<commit_after>#include "lib/config.h"

#if defined(HAVE_R) && !defined(HAVE_SWIG)

#include "interface/RInterface.h"
#include "interface/SGInterface.h"

#include "lib/ShogunException.h"
#include "lib/io.h"

extern "C" {
#include <R.h>
#include <Rinternals.h>
#include <Rdefines.h>
#include <R_ext/Rdynload.h>
}

extern CSGInterface* interface;

CRInterface::CRInterface(SEXP prhs) : CSGInterface()
{
	m_nlhs=0;
	m_nrhs=length(prhs)-1;
	if (m_nrhs<0)
		m_nrhs=0;
	m_lhs=R_NilValue;
	m_rhs=prhs;
}

CRInterface::~CRInterface()
{
}

/** get functions - to pass data from the target interface to shogun */
void CRInterface::parse_args(INT num_args, INT num_default_args)
{
}

/// get type of current argument (does not increment argument counter)
IFType CRInterface::get_argument_type()
{
	SEXP arg=CADR(m_rhs);

	switch (TYPEOF(arg))
	{
		case INTSXP:
			return DENSE_INT;
		case REALSXP:
			return DENSE_REAL;
		case STRSXP:
			return STRING_CHAR;
	};
	return UNDEFINED;
}

INT CRInterface::get_int()
{
	SEXP i=get_arg_increment();
	if (i == R_NilValue || TYPEOF(CAR(i)) != INTSXP || Rf_nrows(CAR(i))!=1 || Rf_ncols(CAR(i))!=1)
		SG_ERROR("Expected Scalar Integer as argument %d\n", m_rhs_counter);

	return INTEGER(CAR(i))[0];
}

DREAL CRInterface::get_real()
{
	SEXP f=get_arg_increment();
	if (f == R_NilValue || TYPEOF(CAR(f)) != REALSXP || Rf_nrows(CAR(f))!=1 || Rf_ncols(CAR(f))!=1)
		SG_ERROR("Expected Scalar Float as argument %d\n", m_rhs_counter);

	return REAL(CAR(f))[0];
}

bool CRInterface::get_bool()
{
	SEXP b=get_arg_increment();
	if (b == R_NilValue || TYPEOF(CAR(b)) != LGLSXP || Rf_nrows(CAR(b))!=1 || Rf_ncols(CAR(b))!=1)
		SG_ERROR("Expected Scalar Boolean as argument %d\n", m_rhs_counter);

	return INTEGER(CAR(b))[0] != 0;
}

CHAR* CRInterface::get_string(INT& len)
{
	SEXP s=get_arg_increment();
	if (s == R_NilValue || TYPEOF(CAR(s)) != STRSXP || length(CAR(s))!=1)
		SG_ERROR("Expected String as argument %d\n", m_rhs_counter);

	SEXPREC* rstr= STRING_ELT(CAR(s),0);
	const CHAR* str= CHAR(rstr);
	len=LENGTH(rstr);
	ASSERT(len>0);
	CHAR* res=new CHAR[len+1];
	memcpy(res, str, len*sizeof(CHAR));
	res[len]='\0';
	return res;
}

void CRInterface::get_byte_vector(BYTE*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_char_vector(CHAR*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_int_vector(INT*& vec, INT& len)
{
	vec=NULL;
	len=0;

	SEXP rvec=CAR(get_arg_increment());
	if( TYPEOF(rvec) != INTSXP )
		SG_ERROR("Expected Integer Vector as argument %d\n", m_rhs_counter);

	len = length(rvec);
	vec=new INT[len];
	ASSERT(vec);

	for (INT i=0; i<len; i++)
		vec[i]= (INT) INTEGER(rvec)[i];
}

void CRInterface::get_shortreal_vector(SHORTREAL*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_real_vector(DREAL*& vec, INT& len)
{
	SEXP rvec=CAR(get_arg_increment());
	if( TYPEOF(rvec) != REALSXP && TYPEOF(rvec) != INTSXP )
		SG_ERROR("Expected Double Vector as argument %d\n", m_rhs_counter);

	len = length(rvec);
	vec=new DREAL[len];
	ASSERT(vec);

	for (INT i=0; i<len; i++)
		vec[i]= (DREAL) REAL(rvec)[i];
}

void CRInterface::get_short_vector(SHORT*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_word_vector(WORD*& vec, INT& len)
{
	vec=NULL;
	len=0;
}

void CRInterface::get_byte_matrix(BYTE*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_char_matrix(CHAR*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_int_matrix(INT*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_shortreal_matrix(SHORTREAL*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_real_matrix(DREAL*& matrix, INT& num_feat, INT& num_vec)
{
	SEXP feat=CAR(get_arg_increment());
	if( TYPEOF(feat) != REALSXP && TYPEOF(feat) != INTSXP )
		SG_ERROR("Expected Double Matrix as argument %d\n", m_rhs_counter);

	num_vec = Rf_ncols(feat);
	num_feat = Rf_nrows(feat);
	matrix=new DREAL[num_vec*num_feat];
	ASSERT(matrix);

	for (INT i=0; i<num_vec; i++)
		for (INT j=0; j<num_feat; j++)
			matrix[i*num_feat+j]= (DREAL) REAL(feat)[i*num_feat+j];
}

void CRInterface::get_short_matrix(SHORT*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_word_matrix(WORD*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_real_sparsematrix(TSparse<DREAL>*& matrix, INT& num_feat, INT& num_vec)
{
}

void CRInterface::get_byte_string_list(T_STRING<BYTE>*& strings, INT& num_str, INT& max_string_len)
{
}

void CRInterface::get_char_string_list(T_STRING<CHAR>*& strings, INT& num_str, INT& max_string_len)
{
	SEXP strs=get_arg_increment();
	//SG_PRINT("len(strs): %d\n", length(CAR(strs))); || length(CAR(strs))>=1
	if (strs == R_NilValue || TYPEOF(CAR(strs)) != STRSXP)
		SG_ERROR("Expected String List as argument %d\n", m_rhs_counter);

	num_str=length(CAR(strs));
	strings=new T_STRING<CHAR>[num_str];
	ASSERT(strings);

	for (int i=0; i<num_str; i++)
	{
		SEXPREC* s= STRING_ELT(strs,i);
		CHAR* c= (CHAR*) CHAR(s);
		int len=LENGTH(s);

		if (len && c)
		{
			CHAR* dst=new CHAR[len];
			strings[i].string=(CHAR*) memcpy(dst, c, len*sizeof(CHAR));
			strings[i].length=len;
		}
		else
		{
			SG_WARNING( "string with index %d has zero length\n", i+1);
			strings[i].string=0;
			strings[i].length=0;
		}
	}
}

void CRInterface::get_int_string_list(T_STRING<INT>*& strings, INT& num_str, INT& max_string_len)
{
}

void CRInterface::get_short_string_list(T_STRING<SHORT>*& strings, INT& num_str, INT& max_string_len)
{
}

void CRInterface::get_word_string_list(T_STRING<WORD>*& strings, INT& num_str, INT& max_string_len)
{
}

/** set functions - to pass data from shogun to the target interface */
bool CRInterface::create_return_values(INT num)
{
	if (num<=0)
		return true;

	PROTECT(m_lhs=allocVector(VECSXP, num));
	m_nlhs=num;
	return length(m_lhs) == num;
}

void CRInterface::set_byte_vector(const BYTE* vec, INT len)
{
}

void CRInterface::set_char_vector(const CHAR* vec, INT len)
{
}

void CRInterface::set_int_vector(const INT* vec, INT len)
{
	SEXP feat=NULL;
	PROTECT( feat = allocVector(INTSXP, len) );

	for (INT i=0; i<len; i++)
		INTEGER(feat)[i]=(int) vec[i];

	UNPROTECT(1);
	set_arg_increment(feat);
}

void CRInterface::set_shortreal_vector(const SHORTREAL* vec, INT len)
{
}

void CRInterface::set_real_vector(const DREAL* vec, INT len)
{
	SEXP feat=NULL;
	PROTECT( feat = allocVector(REALSXP, len) );

	for (INT i=0; i<len; i++)
		REAL(feat)[i]=(double) vec[i];

	UNPROTECT(1);
	set_arg_increment(feat);
}

void CRInterface::set_short_vector(const SHORT* vec, INT len)
{
}

void CRInterface::set_word_vector(const WORD* vec, INT len)
{
}

void CRInterface::set_byte_matrix(const BYTE* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_char_matrix(const CHAR* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_int_matrix(const INT* matrix, INT num_feat, INT num_vec)
{
	SEXP feat=NULL;
	PROTECT( feat = allocMatrix(INTSXP, num_feat, num_vec) );

	for (INT i=0; i<num_vec; i++)
	{
		for (INT j=0; j<num_feat; j++)
			INTEGER(feat)[i*num_feat+j]=(int) matrix[i*num_feat+j];
	}

	UNPROTECT(1);
	set_arg_increment(feat);
}

void CRInterface::set_shortreal_matrix(const SHORTREAL* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_real_matrix(const DREAL* matrix, INT num_feat, INT num_vec)
{
	SEXP feat=NULL;
	PROTECT( feat = allocMatrix(REALSXP, num_feat, num_vec) );

	for (INT i=0; i<num_vec; i++)
	{
		for (INT j=0; j<num_feat; j++)
			REAL(feat)[i*num_feat+j]=(double) matrix[i*num_feat+j];
	}

	UNPROTECT(1);
	set_arg_increment(feat);
}

void CRInterface::set_short_matrix(const SHORT* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_word_matrix(const WORD* matrix, INT num_feat, INT num_vec)
{
}

void CRInterface::set_real_sparsematrix(const TSparse<DREAL>* matrix, INT num_feat, INT num_vec, LONG nnz)
{
	// R does not support sparse matrices yet
}

void CRInterface::set_byte_string_list(const T_STRING<BYTE>* strings, INT num_str)
{
}
 //this function will fail for strings containing 0, unclear how to do 'raw'
 //strings in R
void CRInterface::set_char_string_list(const T_STRING<CHAR>* strings, INT num_str)
{
	if (!strings)
		SG_ERROR("Given strings are invalid.\n");

	SEXP feat=NULL;
	PROTECT( feat = allocVector(STRSXP, num_str) );

	for (INT i=0; i<num_str; i++)
	{
		INT len=strings[i].length;
		if (len>0)
			SET_STRING_ELT(feat, i, mkChar(strings[i].string));
	}
	UNPROTECT(1);
	set_arg_increment(feat);
}

void CRInterface::set_int_string_list(const T_STRING<INT>* strings, INT num_str)
{
}

void CRInterface::set_short_string_list(const T_STRING<SHORT>* strings, INT num_str)
{
}

void CRInterface::set_word_string_list(const T_STRING<WORD>* strings, INT num_str)
{
}

/* The main function of the shogun R interface. All commands from the R command line
 * to the shogun backend are passed using the syntax:
 * .External("sg", "func", ... )
 * where '...' is a number of arguments passed to the shogun function 'func'. */

extern "C" {
/* This method is called by R when the shogun module is loaded into R
 * via dyn.load('sg.so'). */

SEXP sg(SEXP args);

void R_init_sg(DllInfo *info) {

   /* There are four different external language call mechanisms available in R, namely:
    *    .C
    *    .Call
    *    .Fortran
    *    .External
    *
    * Currently shogun uses only the .External interface. */

   R_CMethodDef cMethods[] = { {NULL, NULL, 0} };
   R_FortranMethodDef fortranMethods[] = { {NULL, NULL, 0} };
   R_ExternalMethodDef externalMethods[] = { {"sg", (void*(*)()) &sg, 1}, {NULL, NULL, 0} };
   R_CallMethodDef callMethods[] = { {NULL, NULL, 0} };

   /* Register the routines saved in the callMethods structure so that they are available under R. */
   R_registerRoutines(info, cMethods, callMethods, (R_FortranMethodDef*) fortranMethods, (R_ExternalMethodDef*) externalMethods);

}

SEXP sg(SEXP args)
{
	/* The SEXP (Simple Expression) args is a list of arguments of the .External call.
	 * it consists of "sg", "func" and additional arguments.
	 * */

	delete interface;
	interface=new CRInterface(args);

	try
	{
		if (!interface->handle())
			SG_ERROR("interface currently does not handle this command\n");
	}
	catch (ShogunException e)
	{
		return R_NilValue;
	}

	return ((CRInterface*) interface)->get_return_values();
}

/* This method is called form within R when the current module is unregistered.
 * Note that R does not allow unregistering of single symbols. */

void R_unload_sg(DllInfo *info) { }

} // extern "C"

#endif // HAVE_R && !HAVE_SWIG
<|endoftext|>


<commit_before>#include "fileSystem.h"
#include <json/json.h>
#include <fstream>

std::string util::fileSystem::loadJsonFile(std::string dir)
{
	std::string content;
	std::string line;
	std::ifstream ifs(dir);
	if (!ifs)
	{
		return content;
	}
	while(getline(ifs, line))
	{
		content.append(line);
	}
	return content;
}

Json::Value util::fileSystem::loadJsonFileEval(std::string dir)
{
	std::string content = loadJsonFile(dir);
	Json::Reader reader;
	Json::Value json;
	if (!reader.parse(content, json))
	{
		return Json::nullValue;
	}
	return json;
}<commit_msg>ifstream c98版<commit_after>#include "fileSystem.h"
#include <json/json.h>
#include <fstream>

std::string util::fileSystem::loadJsonFile(std::string dir)
{
	std::string content;
	std::string line;
	std::ifstream ifs(dir.c_str());
	if (!ifs)
	{
		return content;
	}
	while(getline(ifs, line))
	{
		content.append(line);
	}
	return content;
}

Json::Value util::fileSystem::loadJsonFileEval(std::string dir)
{
	std::string content = loadJsonFile(dir);
	Json::Reader reader;
	Json::Value json;
	if (!reader.parse(content, json))
	{
		return Json::nullValue;
	}
	return json;
}<|endoftext|>


<commit_before>/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s):
 * Date:
 *
 * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
 * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include "ShapeMesh.h"
#include "renderer/RenderInterface.h"
#include <iostream>
#include <assimp/Importer.hpp>
#include <assimp/postprocess.h>
#include <assimp/cimport.h>

using namespace Eigen;
using namespace std;

namespace kinematics {

    ShapeMesh::ShapeMesh(Vector3d _dim, double _mass, const aiScene *_mesh){
        mType = P_MESH;
        mDim = _dim;
        mMass = _mass;
        mMeshData = _mesh;
        initMeshes();
        if (mDim != Vector3d::Zero())
            computeVolume();
        if (mMass != 0){
            computeMassTensor();
            computeInertiaFromMassTensor();
        }
    }

    void ShapeMesh::draw(renderer::RenderInterface* _ri, const Vector4d& _color, bool _useDefaultColor) const {
        if (!_ri)
            return;
        if (!_useDefaultColor)
            _ri->setPenColor(_color);
        else
            _ri->setPenColor(mColor);
        _ri->pushMatrix();
        _ri->drawMesh(mDim, mVizMesh);
        _ri->popMatrix();
    }

    void ShapeMesh::computeMassTensor() {
        // use bounding box to represent the mesh
        double max_X = -1e7;
        double max_Y = -1e7;
        double max_Z = -1e7;
        double min_X = 1e7;
        double min_Y = 1e7;
        double min_Z = 1e7;

        for(unsigned int i = 0; i < mMeshData->mNumMeshes; i++) {
            for (unsigned int j = 0; j < mMeshData->mMeshes[i]->mNumVertices; j++) {
                if (mMeshData->mMeshes[i]->mVertices[j].x > max_X)
                    max_X = mMeshData->mMeshes[i]->mVertices[j].x;
                if (mMeshData->mMeshes[i]->mVertices[j].x < min_X)
                    min_X = mMeshData->mMeshes[i]->mVertices[j].x;
                if (mMeshData->mMeshes[i]->mVertices[j].y > max_Y)
                    max_Y = mMeshData->mMeshes[i]->mVertices[j].y;
                if (mMeshData->mMeshes[i]->mVertices[j].y < min_Y)
                    min_Y = mMeshData->mMeshes[i]->mVertices[j].y;
                if (mMeshData->mMeshes[i]->mVertices[j].z > max_Z)
                    max_Z = mMeshData->mMeshes[i]->mVertices[j].z;
                if (mMeshData->mMeshes[i]->mVertices[j].z < min_Z)
                    min_Z = mMeshData->mMeshes[i]->mVertices[j].z;
            }
        }
        double l = mDim[0] * (max_X - min_X);
        double h = mDim[1] * (max_Y - min_Y);
        double w = mDim[2] * (max_Z - min_Z);

        mMassTensor(0, 0) = l * l / 12;
        mMassTensor(1, 1) = h * h / 12;
        mMassTensor(2, 2) = w * w / 12;
        mMassTensor(3, 3) = 1;
        mMassTensor *= mMass;
    }

    void ShapeMesh::computeVolume() {
        mVolume = mDim(0) * mDim(1) * mDim(2); // a * b * c
    }

    void ShapeMesh::initMeshes() {
    }

    const aiScene* ShapeMesh::loadMesh(const string& fileName) {
        aiPropertyStore* propertyStore = aiCreatePropertyStore();
        aiSetImportPropertyInteger(propertyStore, AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_POINT | aiPrimitiveType_LINE ); // remove points and lines
        const aiScene* scene = aiImportFileExWithProperties(fileName.c_str(), aiProcess_GenNormals             |
                                                                              aiProcess_Triangulate            |
                                                                              aiProcess_JoinIdenticalVertices  |
                                                                              aiProcess_SortByPType            |
                                                                              aiProcess_PreTransformVertices   |
                                                                              aiProcess_OptimizeMeshes,
                                                            NULL, propertyStore);
        aiReleasePropertyStore(propertyStore);
        return scene;
    }
} // namespace kinematics
<commit_msg>Fix ShapeMesh::loadMesh()<commit_after>/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s):
 * Date:
 *
 * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
 * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include "ShapeMesh.h"
#include "renderer/RenderInterface.h"
#include <iostream>
#include <assimp/Importer.hpp>
#include <assimp/postprocess.h>
#include <assimp/cimport.h>

using namespace Eigen;
using namespace std;

namespace kinematics {

    ShapeMesh::ShapeMesh(Vector3d _dim, double _mass, const aiScene *_mesh){
        mType = P_MESH;
        mDim = _dim;
        mMass = _mass;
        mMeshData = _mesh;
        initMeshes();
        if (mDim != Vector3d::Zero())
            computeVolume();
        if (mMass != 0){
            computeMassTensor();
            computeInertiaFromMassTensor();
        }
    }

    void ShapeMesh::draw(renderer::RenderInterface* _ri, const Vector4d& _color, bool _useDefaultColor) const {
        if (!_ri)
            return;
        if (!_useDefaultColor)
            _ri->setPenColor(_color);
        else
            _ri->setPenColor(mColor);
        _ri->pushMatrix();
        _ri->drawMesh(mDim, mVizMesh);
        _ri->popMatrix();
    }

    void ShapeMesh::computeMassTensor() {
        // use bounding box to represent the mesh
        double max_X = -1e7;
        double max_Y = -1e7;
        double max_Z = -1e7;
        double min_X = 1e7;
        double min_Y = 1e7;
        double min_Z = 1e7;

        for(unsigned int i = 0; i < mMeshData->mNumMeshes; i++) {
            for (unsigned int j = 0; j < mMeshData->mMeshes[i]->mNumVertices; j++) {
                if (mMeshData->mMeshes[i]->mVertices[j].x > max_X)
                    max_X = mMeshData->mMeshes[i]->mVertices[j].x;
                if (mMeshData->mMeshes[i]->mVertices[j].x < min_X)
                    min_X = mMeshData->mMeshes[i]->mVertices[j].x;
                if (mMeshData->mMeshes[i]->mVertices[j].y > max_Y)
                    max_Y = mMeshData->mMeshes[i]->mVertices[j].y;
                if (mMeshData->mMeshes[i]->mVertices[j].y < min_Y)
                    min_Y = mMeshData->mMeshes[i]->mVertices[j].y;
                if (mMeshData->mMeshes[i]->mVertices[j].z > max_Z)
                    max_Z = mMeshData->mMeshes[i]->mVertices[j].z;
                if (mMeshData->mMeshes[i]->mVertices[j].z < min_Z)
                    min_Z = mMeshData->mMeshes[i]->mVertices[j].z;
            }
        }
        double l = mDim[0] * (max_X - min_X);
        double h = mDim[1] * (max_Y - min_Y);
        double w = mDim[2] * (max_Z - min_Z);

        mMassTensor(0, 0) = l * l / 12;
        mMassTensor(1, 1) = h * h / 12;
        mMassTensor(2, 2) = w * w / 12;
        mMassTensor(3, 3) = 1;
        mMassTensor *= mMass;
    }

    void ShapeMesh::computeVolume() {
        mVolume = mDim(0) * mDim(1) * mDim(2); // a * b * c
    }

    void ShapeMesh::initMeshes() {
    }

    const aiScene* ShapeMesh::loadMesh(const string& fileName) {
        aiPropertyStore* propertyStore = aiCreatePropertyStore();
        aiSetImportPropertyInteger(propertyStore, AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_POINT | aiPrimitiveType_LINE ); // remove points and lines
        const aiScene* scene = aiImportFileExWithProperties(fileName.c_str(), aiProcess_GenNormals             |
                                                                              aiProcess_Triangulate            |
                                                                              aiProcess_JoinIdenticalVertices  |
                                                                              aiProcess_SortByPType            |
                                                                              aiProcess_OptimizeMeshes,
                                                            NULL, propertyStore);
        aiReleasePropertyStore(propertyStore);

        // Assimp rotates collada files such that the up-axis (specified in the
        // collada file) aligns with assimp's y-axis. Here we are reverting this
        // rotation. We are only catching files with the .dae file ending here. We
        // might miss files with an .xml file ending, which would need to be looked
        // into to figure out whether they are collada files.
        if (fileName.length() >= 4 && fileName.substr(fileName.length() - 4, 4) == ".dae")
          scene->mRootNode->mTransformation = aiMatrix4x4();
        scene = aiApplyPostProcessing(scene, aiProcess_PreTransformVertices);

        return scene;
    }
} // namespace kinematics
<|endoftext|>