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

#include "modules/map/relative_map/navigation_lane.h"

#include <algorithm>
#include <limits>

#include "modules/map/proto/map_lane.pb.h"

#include "modules/common/log.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/util.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/map/relative_map/common/relative_map_gflags.h"

namespace apollo {
namespace relative_map {

using apollo::common::VehicleStateProvider;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;
using apollo::hdmap::Lane;
using apollo::common::util::operator+;
using apollo::perception::PerceptionObstacles;

NavigationLane::NavigationLane(const NavigationLaneConfig &config)
    : config_(config) {}

void NavigationLane::SetConfig(const NavigationLaneConfig &config) {
  config_ = config;
}

bool NavigationLane::GeneratePath() {
  // original_pose is in world coordination: ENU
  original_pose_ = VehicleStateProvider::instance()->original_pose();

  navigation_path_.Clear();
  auto *path = navigation_path_.mutable_path();
  const auto &lane_marker = perception_obstacles_.lane_marker();

  // priority: merge > navigation line > perception lane marker
  if (FLAGS_enable_navigation_line &&
      navigation_info_.navigation_path_size() > 0) {
    ConvertNavigationLineToPath(path);
    if (path->path_point().size() <= 0) {
      ConvertLaneMarkerToPath(lane_marker, path);
    }
  } else {
    ConvertLaneMarkerToPath(lane_marker, path);
  }
  return true;
}

double NavigationLane::EvaluateCubicPolynomial(const double c0, const double c1,
                                               const double c2, const double c3,
                                               const double z) const {
  return ((c3 * z + c2) * z + c1) * z + c0;
}

void NavigationLane::MergeNavigationLineAndLaneMarker(
    const perception::LaneMarkers &lane_marker, common::Path *path) {
  CHECK_NOTNULL(path);

  common::Path navigation_path;
  ConvertNavigationLineToPath(&navigation_path);

  common::Path lane_marker_path;
  ConvertLaneMarkerToPath(perception_obstacles_.lane_marker(),
                          &lane_marker_path);

  const double len = std::fmin(
      navigation_path.path_point(navigation_path.path_point_size() - 1).s(),
      lane_marker_path.path_point(lane_marker_path.path_point_size() - 1).s());

  const double ds = 1.0;
  int navigation_index = 0;
  int lane_marker_index = 0;
  for (double s = 0.0; s < len; s += ds) {
    auto p1 = GetPathPointByS(navigation_path, navigation_index, s,
                              &navigation_index);
    auto p2 = GetPathPointByS(lane_marker_path, lane_marker_index, s,
                              &lane_marker_index);
    auto *p = path->add_path_point();
    const double kWeight = 0.9;
    *p = common::util::GetWeightedAverageOfTwoPathPoints(p1, p2, kWeight,
                                                         1 - kWeight);
  }
}

common::PathPoint NavigationLane::GetPathPointByS(const common::Path &path,
                                                  const int start_index,
                                                  const double s,
                                                  int *matched_index) {
  CHECK_NOTNULL(matched_index);

  constexpr double kEpsilon = 1e-9;
  if (std::fabs(path.path_point(start_index).s() - s) < kEpsilon) {
    *matched_index = start_index;
    return path.path_point(start_index);
  }
  int i = start_index;
  while (i + 1 < path.path_point_size() && path.path_point(i + 1).s() < s) {
    ++i;
  }
  *matched_index = i;

  // x, y, z, theta, kappa, s, dkappa, ddkappa
  const double r = (s - path.path_point(i).s()) /
                   (path.path_point(i + 1).s() - path.path_point(i).s());
  auto p = common::util::GetWeightedAverageOfTwoPathPoints(
      path.path_point(i), path.path_point(i + 1), 1 - r, r);
  return p;
}

void NavigationLane::ConvertNavigationLineToPath(common::Path *path) {
  CHECK_NOTNULL(path);
  if (navigation_info_.navigation_path_size() == 0 ||
      !navigation_info_.navigation_path(0).has_path() ||
      navigation_info_.navigation_path(0).path().path_point_size() == 0) {
    // path is empty
    return;
  }
  path->set_name("Path from navigation.");
  UpdateProjectionIndex();

  // TODO(All): support multiple navigation path
  // currently, only 1 navigation path is supported
  const auto &navigation_path = navigation_info_.navigation_path(0).path();
  int curr_project_index = last_project_index_;

  double dist =
      navigation_path.path_point(navigation_path.path_point_size() - 1).s() -
      navigation_path.path_point(curr_project_index).s();
  if (dist < 20) {
    return;
  }

  // offset between the current vehicle state and navigation line
  const double dx = -original_pose_.position().x();
  const double dy = -original_pose_.position().y();
  const double ref_s = navigation_path.path_point(curr_project_index).s();
  for (int i = std::max(0, curr_project_index - 3);
       i < navigation_path.path_point_size(); ++i) {
    auto *point = path->add_path_point();
    point->CopyFrom(navigation_path.path_point(i));

    // shift to (0, 0)
    double enu_x = point->x() + dx;
    double enu_y = point->y() + dy;

    double flu_x = 0.0;
    double flu_y = 0.0;
    common::math::RotateAxis(original_pose_.heading(), enu_x, enu_y, &flu_x,
                             &flu_y);

    point->set_x(flu_x);
    point->set_y(flu_y);
    point->set_theta(point->theta() - original_pose_.heading());
    const double accumulated_s = navigation_path.path_point(i).s() - ref_s;
    point->set_s(accumulated_s);

    if (accumulated_s > FLAGS_max_len_from_navigation_line) {
      break;
    }
  }

  const perception::LaneMarkers &lane_marker =
      perception_obstacles_.lane_marker();
  const auto &left_lane = lane_marker.left_lane_marker();
  const auto &right_lane = lane_marker.right_lane_marker();
  left_width_ = (std::fabs(left_lane.c0_position()) +
                 std::fabs(right_lane.c0_position())) /
                2.0;
  right_width_ = left_width_;
}

// project adc_state_ onto path
void NavigationLane::UpdateProjectionIndex() {
  // TODO(All): support multiple navigation path
  // currently, only 1 navigation path is supported
  const auto &path = navigation_info_.navigation_path(0).path();
  int index = 0;
  double min_d = std::numeric_limits<double>::max();
  for (int i = last_project_index_; i + 1 < path.path_point_size(); ++i) {
    const double d = DistanceXY(original_pose_.position(), path.path_point(i));
    if (d < min_d) {
      min_d = d;
      index = i;
    }
    const double kMaxDistance = 50.0;
    if (last_project_index_ != 0 && d > kMaxDistance) {
      break;
    }
  }
  last_project_index_ = index;
}

void NavigationLane::ConvertLaneMarkerToPath(
    const perception::LaneMarkers &lane_marker, common::Path *path) {
  CHECK_NOTNULL(path);

  path->set_name("Path from lane markers.");
  const auto &left_lane = lane_marker.left_lane_marker();
  const auto &right_lane = lane_marker.right_lane_marker();

  double path_c0 = (left_lane.c0_position() + right_lane.c0_position()) / 2.0;

  double left_quality = left_lane.quality() + 0.001;
  double right_quality = right_lane.quality() + 0.001;

  double quality_divider = left_quality + right_quality;

  double path_c1 = (left_lane.c1_heading_angle() * left_quality +
                    right_lane.c1_heading_angle() * right_quality) /
                   quality_divider;

  double path_c2 = (left_lane.c2_curvature() * left_quality +
                    right_lane.c2_curvature() * right_quality) /
                   quality_divider;

  double path_c3 =
      (left_lane.c3_curvature_derivative() * left_quality +
       right_lane.c3_curvature_derivative() * right_quality) /
      quality_divider;

  const double current_speed =
      VehicleStateProvider::instance()->vehicle_state().linear_velocity();
  double path_range = current_speed * FLAGS_ratio_navigation_lane_len_to_speed;
  if (path_range <= FLAGS_min_len_for_navigation_lane) {
    path_range = FLAGS_min_len_for_navigation_lane;
  } else {
    path_range = FLAGS_max_len_for_navigation_lane;
  }

  const double unit_z = 1.0;
  const double start_s = -2.0;
  double accumulated_s = start_s;
  for (double z = start_s; z <= path_range; z += unit_z) {
    double x_l = EvaluateCubicPolynomial(path_c0, path_c1, path_c2, path_c3, z);

    double x1 = z;
    double y1 = x_l;

    auto *point = path->add_path_point();
    point->set_x(x1);
    point->set_y(y1);

    if (path->path_point_size() > 1) {
      auto &pre_point = path->path_point(path->path_point_size() - 2);
      accumulated_s += std::hypot(x1 - pre_point.x(), y1 - pre_point.y());
    }
    point->set_s(accumulated_s);
  }

  left_width_ = (std::fabs(left_lane.c0_position()) +
                 std::fabs(right_lane.c0_position())) /
                2.0;
  right_width_ = left_width_;
}

bool NavigationLane::CreateMap(const MapGenerationParam &map_config,
                               MapMsg *map_msg) const {
  auto *navigation_info = map_msg->mutable_navigation_path();
  auto *hdmap = map_msg->mutable_hdmap();
  auto *lane_marker = map_msg->mutable_lane_marker();

  lane_marker->CopyFrom(perception_obstacles_.lane_marker());

  const auto &path = navigation_path_.path();
  if (path.path_point_size() < 2) {
    AERROR << "The path length is invalid";
    return false;
  }
  auto *lane = hdmap->add_lane();
  lane->mutable_id()->set_id(std::to_string(navigation_path_.path_priority()) +
                             "_" + path.name());
  (*navigation_info)[lane->id().id()] = navigation_path_;
  // lane types
  lane->set_type(Lane::CITY_DRIVING);
  lane->set_turn(Lane::NO_TURN);

  // speed limit
  lane->set_speed_limit(map_config.default_speed_limit());

  // center line
  auto *curve_segment = lane->mutable_central_curve()->add_segment();
  curve_segment->set_heading(path.path_point(0).theta());
  auto *line_segment = curve_segment->mutable_line_segment();
  // left boundary
  auto *left_boundary = lane->mutable_left_boundary();
  auto *left_boundary_type = left_boundary->add_boundary_type();
  left_boundary->set_virtual_(false);
  left_boundary_type->set_s(0.0);
  left_boundary_type->add_types(
      perception_obstacles_.lane_marker().left_lane_marker().lane_type());
  auto *left_segment =
      left_boundary->mutable_curve()->add_segment()->mutable_line_segment();
  // right boundary
  auto *right_boundary = lane->mutable_right_boundary();
  auto *right_boundary_type = right_boundary->add_boundary_type();
  right_boundary->set_virtual_(false);
  right_boundary_type->set_s(0.0);
  right_boundary_type->add_types(
      perception_obstacles_.lane_marker().right_lane_marker().lane_type());
  auto *right_segment =
      right_boundary->mutable_curve()->add_segment()->mutable_line_segment();
  const double lane_left_width =
      left_width_ > 0 ? left_width_ : map_config.default_left_width();
  const double lane_right_width =
      right_width_ > 0 ? right_width_ : map_config.default_right_width();
  for (const auto &path_point : path.path_point()) {
    auto *point = line_segment->add_point();
    point->set_x(path_point.x());
    point->set_y(path_point.y());
    point->set_z(path_point.z());
    auto *left_sample = lane->add_left_sample();
    left_sample->set_s(path_point.s());
    left_sample->set_width(lane_left_width);
    left_segment->add_point()->CopyFrom(
        *point +
        lane_left_width * Vec2d::CreateUnitVec2d(path_point.theta() + M_PI_2));
    auto *right_sample = lane->add_right_sample();
    right_sample->set_s(path_point.s());
    right_sample->set_width(lane_right_width);
    right_segment->add_point()->CopyFrom(
        *point +
        lane_right_width * Vec2d::CreateUnitVec2d(path_point.theta() - M_PI_2));
  }

  return true;
}

}  // namespace relative_map
}  // namespace apollo
<commit_msg>navi: fixed the heading issue in navi lane.<commit_after>/******************************************************************************
 * Copyright 2018 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.
 *****************************************************************************/

#include "modules/map/relative_map/navigation_lane.h"

#include <algorithm>
#include <limits>

#include "modules/map/proto/map_lane.pb.h"

#include "modules/common/log.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/util.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/map/relative_map/common/relative_map_gflags.h"

namespace apollo {
namespace relative_map {

using apollo::common::VehicleStateProvider;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;
using apollo::hdmap::Lane;
using apollo::common::util::operator+;
using apollo::perception::PerceptionObstacles;

NavigationLane::NavigationLane(const NavigationLaneConfig &config)
    : config_(config) {}

void NavigationLane::SetConfig(const NavigationLaneConfig &config) {
  config_ = config;
}

bool NavigationLane::GeneratePath() {
  // original_pose is in world coordination: ENU
  original_pose_ = VehicleStateProvider::instance()->original_pose();

  navigation_path_.Clear();
  auto *path = navigation_path_.mutable_path();
  const auto &lane_marker = perception_obstacles_.lane_marker();

  // priority: merge > navigation line > perception lane marker
  if (FLAGS_enable_navigation_line &&
      navigation_info_.navigation_path_size() > 0) {
    ConvertNavigationLineToPath(path);
    if (path->path_point().size() <= 0) {
      ConvertLaneMarkerToPath(lane_marker, path);
    }
  } else {
    ConvertLaneMarkerToPath(lane_marker, path);
  }
  return true;
}

double NavigationLane::EvaluateCubicPolynomial(const double c0, const double c1,
                                               const double c2, const double c3,
                                               const double z) const {
  return ((c3 * z + c2) * z + c1) * z + c0;
}

void NavigationLane::MergeNavigationLineAndLaneMarker(
    const perception::LaneMarkers &lane_marker, common::Path *path) {
  CHECK_NOTNULL(path);

  common::Path navigation_path;
  ConvertNavigationLineToPath(&navigation_path);

  common::Path lane_marker_path;
  ConvertLaneMarkerToPath(perception_obstacles_.lane_marker(),
                          &lane_marker_path);

  const double len = std::fmin(
      navigation_path.path_point(navigation_path.path_point_size() - 1).s(),
      lane_marker_path.path_point(lane_marker_path.path_point_size() - 1).s());

  const double ds = 1.0;
  int navigation_index = 0;
  int lane_marker_index = 0;
  for (double s = 0.0; s < len; s += ds) {
    auto p1 = GetPathPointByS(navigation_path, navigation_index, s,
                              &navigation_index);
    auto p2 = GetPathPointByS(lane_marker_path, lane_marker_index, s,
                              &lane_marker_index);
    auto *p = path->add_path_point();
    const double kWeight = 0.9;
    *p = common::util::GetWeightedAverageOfTwoPathPoints(p1, p2, kWeight,
                                                         1 - kWeight);
  }
}

common::PathPoint NavigationLane::GetPathPointByS(const common::Path &path,
                                                  const int start_index,
                                                  const double s,
                                                  int *matched_index) {
  CHECK_NOTNULL(matched_index);

  constexpr double kEpsilon = 1e-9;
  if (std::fabs(path.path_point(start_index).s() - s) < kEpsilon) {
    *matched_index = start_index;
    return path.path_point(start_index);
  }
  int i = start_index;
  while (i + 1 < path.path_point_size() && path.path_point(i + 1).s() < s) {
    ++i;
  }
  *matched_index = i;

  // x, y, z, theta, kappa, s, dkappa, ddkappa
  const double r = (s - path.path_point(i).s()) /
                   (path.path_point(i + 1).s() - path.path_point(i).s());
  auto p = common::util::GetWeightedAverageOfTwoPathPoints(
      path.path_point(i), path.path_point(i + 1), 1 - r, r);
  return p;
}

void NavigationLane::ConvertNavigationLineToPath(common::Path *path) {
  CHECK_NOTNULL(path);
  if (navigation_info_.navigation_path_size() == 0 ||
      !navigation_info_.navigation_path(0).has_path() ||
      navigation_info_.navigation_path(0).path().path_point_size() == 0) {
    // path is empty
    return;
  }
  path->set_name("Path from navigation.");
  UpdateProjectionIndex();

  // TODO(All): support multiple navigation path
  // currently, only 1 navigation path is supported
  const auto &navigation_path = navigation_info_.navigation_path(0).path();
  int curr_project_index = last_project_index_;

  double dist =
      navigation_path.path_point(navigation_path.path_point_size() - 1).s() -
      navigation_path.path_point(curr_project_index).s();
  if (dist < 20) {
    return;
  }

  // offset between the current vehicle state and navigation line
  const double dx = -original_pose_.position().x();
  const double dy = -original_pose_.position().y();
  const double ref_s = navigation_path.path_point(curr_project_index).s();
  for (int i = std::max(0, curr_project_index - 3);
       i < navigation_path.path_point_size(); ++i) {
    auto *point = path->add_path_point();
    point->CopyFrom(navigation_path.path_point(i));

    // shift to (0, 0)
    double enu_x = point->x() + dx;
    double enu_y = point->y() + dy;

    double flu_x = 0.0;
    double flu_y = 0.0;
    common::math::RotateAxis(original_pose_.heading(), enu_x, enu_y, &flu_x,
                             &flu_y);

    point->set_x(flu_x);
    point->set_y(flu_y);
    point->set_theta(common::math::NormalizeAngle(
        common::math::NormalizeAngle(point->theta())
            - original_pose_.heading()));
    const double accumulated_s = navigation_path.path_point(i).s() - ref_s;
    point->set_s(accumulated_s);

    if (accumulated_s > FLAGS_max_len_from_navigation_line) {
      break;
    }
  }

  const perception::LaneMarkers &lane_marker =
      perception_obstacles_.lane_marker();
  const auto &left_lane = lane_marker.left_lane_marker();
  const auto &right_lane = lane_marker.right_lane_marker();
  left_width_ = (std::fabs(left_lane.c0_position()) +
                 std::fabs(right_lane.c0_position())) /
                2.0;
  right_width_ = left_width_;
}

// project adc_state_ onto path
void NavigationLane::UpdateProjectionIndex() {
  // TODO(All): support multiple navigation path
  // currently, only 1 navigation path is supported
  const auto &path = navigation_info_.navigation_path(0).path();
  int index = 0;
  double min_d = std::numeric_limits<double>::max();
  for (int i = last_project_index_; i + 1 < path.path_point_size(); ++i) {
    const double d = DistanceXY(original_pose_.position(), path.path_point(i));
    if (d < min_d) {
      min_d = d;
      index = i;
    }
    const double kMaxDistance = 50.0;
    if (last_project_index_ != 0 && d > kMaxDistance) {
      break;
    }
  }
  last_project_index_ = index;
}

void NavigationLane::ConvertLaneMarkerToPath(
    const perception::LaneMarkers &lane_marker, common::Path *path) {
  CHECK_NOTNULL(path);

  path->set_name("Path from lane markers.");
  const auto &left_lane = lane_marker.left_lane_marker();
  const auto &right_lane = lane_marker.right_lane_marker();

  double path_c0 = (left_lane.c0_position() + right_lane.c0_position()) / 2.0;

  double left_quality = left_lane.quality() + 0.001;
  double right_quality = right_lane.quality() + 0.001;

  double quality_divider = left_quality + right_quality;

  double path_c1 = (left_lane.c1_heading_angle() * left_quality +
                    right_lane.c1_heading_angle() * right_quality) /
                   quality_divider;

  double path_c2 = (left_lane.c2_curvature() * left_quality +
                    right_lane.c2_curvature() * right_quality) /
                   quality_divider;

  double path_c3 =
      (left_lane.c3_curvature_derivative() * left_quality +
       right_lane.c3_curvature_derivative() * right_quality) /
      quality_divider;

  const double current_speed =
      VehicleStateProvider::instance()->vehicle_state().linear_velocity();
  double path_range = current_speed * FLAGS_ratio_navigation_lane_len_to_speed;
  if (path_range <= FLAGS_min_len_for_navigation_lane) {
    path_range = FLAGS_min_len_for_navigation_lane;
  } else {
    path_range = FLAGS_max_len_for_navigation_lane;
  }

  const double unit_z = 1.0;
  const double start_s = -2.0;
  double accumulated_s = start_s;
  for (double z = start_s; z <= path_range; z += unit_z) {
    double x_l = EvaluateCubicPolynomial(path_c0, path_c1, path_c2, path_c3, z);

    double x1 = z;
    double y1 = x_l;

    auto *point = path->add_path_point();
    point->set_x(x1);
    point->set_y(y1);

    if (path->path_point_size() > 1) {
      auto &pre_point = path->path_point(path->path_point_size() - 2);
      accumulated_s += std::hypot(x1 - pre_point.x(), y1 - pre_point.y());
    }
    point->set_s(accumulated_s);
  }

  left_width_ = (std::fabs(left_lane.c0_position()) +
                 std::fabs(right_lane.c0_position())) /
                2.0;
  right_width_ = left_width_;
}

bool NavigationLane::CreateMap(const MapGenerationParam &map_config,
                               MapMsg *map_msg) const {
  auto *navigation_info = map_msg->mutable_navigation_path();
  auto *hdmap = map_msg->mutable_hdmap();
  auto *lane_marker = map_msg->mutable_lane_marker();

  lane_marker->CopyFrom(perception_obstacles_.lane_marker());

  const auto &path = navigation_path_.path();
  if (path.path_point_size() < 2) {
    AERROR << "The path length is invalid";
    return false;
  }
  auto *lane = hdmap->add_lane();
  lane->mutable_id()->set_id(std::to_string(navigation_path_.path_priority()) +
                             "_" + path.name());
  (*navigation_info)[lane->id().id()] = navigation_path_;
  // lane types
  lane->set_type(Lane::CITY_DRIVING);
  lane->set_turn(Lane::NO_TURN);

  // speed limit
  lane->set_speed_limit(map_config.default_speed_limit());

  // center line
  auto *curve_segment = lane->mutable_central_curve()->add_segment();
  curve_segment->set_heading(path.path_point(0).theta());
  auto *line_segment = curve_segment->mutable_line_segment();
  // left boundary
  auto *left_boundary = lane->mutable_left_boundary();
  auto *left_boundary_type = left_boundary->add_boundary_type();
  left_boundary->set_virtual_(false);
  left_boundary_type->set_s(0.0);
  left_boundary_type->add_types(
      perception_obstacles_.lane_marker().left_lane_marker().lane_type());
  auto *left_segment =
      left_boundary->mutable_curve()->add_segment()->mutable_line_segment();
  // right boundary
  auto *right_boundary = lane->mutable_right_boundary();
  auto *right_boundary_type = right_boundary->add_boundary_type();
  right_boundary->set_virtual_(false);
  right_boundary_type->set_s(0.0);
  right_boundary_type->add_types(
      perception_obstacles_.lane_marker().right_lane_marker().lane_type());
  auto *right_segment =
      right_boundary->mutable_curve()->add_segment()->mutable_line_segment();
  const double lane_left_width =
      left_width_ > 0 ? left_width_ : map_config.default_left_width();
  const double lane_right_width =
      right_width_ > 0 ? right_width_ : map_config.default_right_width();
  for (const auto &path_point : path.path_point()) {
    auto *point = line_segment->add_point();
    point->set_x(path_point.x());
    point->set_y(path_point.y());
    point->set_z(path_point.z());
    auto *left_sample = lane->add_left_sample();
    left_sample->set_s(path_point.s());
    left_sample->set_width(lane_left_width);
    left_segment->add_point()->CopyFrom(
        *point +
        lane_left_width * Vec2d::CreateUnitVec2d(path_point.theta() + M_PI_2));
    auto *right_sample = lane->add_right_sample();
    right_sample->set_s(path_point.s());
    right_sample->set_width(lane_right_width);
    right_segment->add_point()->CopyFrom(
        *point +
        lane_right_width * Vec2d::CreateUnitVec2d(path_point.theta() - M_PI_2));
  }

  return true;
}

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


<commit_before>/**
 * \file
 * \brief SpiPeripheral class header for SPIv2 in STM32
 *
 * \author Copyright (C) 2016-2018 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef SOURCE_CHIP_STM32_PERIPHERALS_SPIV2_INCLUDE_DISTORTOS_CHIP_STM32_SPIV2_SPIPERIPHERAL_HPP_
#define SOURCE_CHIP_STM32_PERIPHERALS_SPIV2_INCLUDE_DISTORTOS_CHIP_STM32_SPIV2_SPIPERIPHERAL_HPP_

#include "distortos/chip/getBusFrequency.hpp"

namespace distortos
{

namespace chip
{

/// SpiPeripheral class is a raw SPI peripheral for SPIv2 in STM32
class SpiPeripheral
{
public:

	/**
	 * \brief SpiPeripheral's constructor
	 *
	 * \param [in] spiBase is a base address of SPI peripheral
	 */

	constexpr explicit SpiPeripheral(const uintptr_t spiBase) :
			spiBase_{spiBase},
			peripheralFrequency_{getBusFrequency(spiBase)}
	{

	}

	/**
	 * \return peripheral clock frequency, Hz
	 */

	uint32_t getPeripheralFrequency() const
	{
		return peripheralFrequency_;
	}

	/**
	 * \return reference to SPI_TypeDef object
	 */

	SPI_TypeDef& getSpi() const
	{
		return *reinterpret_cast<SPI_TypeDef*>(spiBase_);
	}

	/**
	 * \return current value of CR1 register
	 */

	uint32_t readCr1() const
	{
		return getSpi().CR1;
	}

	/**
	 * \return current value of CR2 register
	 */

	uint32_t readCr2() const
	{
		return getSpi().CR2;
	}

	/**
	 * \brief Reads current value of DR register.
	 *
	 * \param [in] wordLength selects word length, bits, [4; 16] or
	 * [ChipSpiMasterLowLevel::minWordLength; ChipSpiMasterLowLevel::maxWordLength]
	 *
	 * \return current value of DR register
	 */

	uint32_t readDr(const uint8_t wordLength) const
	{
		return wordLength <= 8 ? *reinterpret_cast<volatile uint8_t*>(&getSpi().DR) : getSpi().DR;
	}

	/**
	 * \return current value of SR register
	 */

	uint32_t readSr() const
	{
		return getSpi().SR;
	}

	/**
	 * \brief Writes value to CR1 register.
	 *
	 * \param [in] cr1 is the value that will be written to CR1 register
	 */

	void writeCr1(const uint32_t cr1) const
	{
		getSpi().CR1 = cr1;
	}

	/**
	 * \brief Writes value to CR2 register.
	 *
	 * \param [in] cr2 is the value that will be written to CR2 register
	 */

	void writeCr2(const uint32_t cr2) const
	{
		getSpi().CR2 = cr2;
	}

	/**
	 * \brief Writes value to DR register.
	 *
	 * \param [in] wordLength selects word length, bits, [4; 16] or
	 * [ChipSpiMasterLowLevel::minWordLength; ChipSpiMasterLowLevel::maxWordLength]
	 * \param [in] dr is the value that will be written to DR register
	 */

	void writeDr(const uint8_t wordLength, const uint32_t dr) const
	{
		if (wordLength <= 8)
			*reinterpret_cast<volatile uint8_t*>(&getSpi().DR) = dr;
		else
			getSpi().DR = dr;
	}

	/**
	 * \brief Writes value to SR register.
	 *
	 * \param [in] sr is the value that will be written to SR register
	 */

	void writeSr(const uint32_t sr) const
	{
		getSpi().SR = sr;
	}

private:

	/// base address of SPI peripheral
	uintptr_t spiBase_;

	/// peripheral clock frequency, Hz
	uint32_t peripheralFrequency_;
};

}	// namespace chip

}	// namespace distortos

#endif	// SOURCE_CHIP_STM32_PERIPHERALS_SPIV2_INCLUDE_DISTORTOS_CHIP_STM32_SPIV2_SPIPERIPHERAL_HPP_
<commit_msg>Make STM32 SPIv2 SpiPeripheral::getSpi() private<commit_after>/**
 * \file
 * \brief SpiPeripheral class header for SPIv2 in STM32
 *
 * \author Copyright (C) 2016-2018 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef SOURCE_CHIP_STM32_PERIPHERALS_SPIV2_INCLUDE_DISTORTOS_CHIP_STM32_SPIV2_SPIPERIPHERAL_HPP_
#define SOURCE_CHIP_STM32_PERIPHERALS_SPIV2_INCLUDE_DISTORTOS_CHIP_STM32_SPIV2_SPIPERIPHERAL_HPP_

#include "distortos/chip/getBusFrequency.hpp"

namespace distortos
{

namespace chip
{

/// SpiPeripheral class is a raw SPI peripheral for SPIv2 in STM32
class SpiPeripheral
{
public:

	/**
	 * \brief SpiPeripheral's constructor
	 *
	 * \param [in] spiBase is a base address of SPI peripheral
	 */

	constexpr explicit SpiPeripheral(const uintptr_t spiBase) :
			spiBase_{spiBase},
			peripheralFrequency_{getBusFrequency(spiBase)}
	{

	}

	/**
	 * \return peripheral clock frequency, Hz
	 */

	uint32_t getPeripheralFrequency() const
	{
		return peripheralFrequency_;
	}

	/**
	 * \return current value of CR1 register
	 */

	uint32_t readCr1() const
	{
		return getSpi().CR1;
	}

	/**
	 * \return current value of CR2 register
	 */

	uint32_t readCr2() const
	{
		return getSpi().CR2;
	}

	/**
	 * \brief Reads current value of DR register.
	 *
	 * \param [in] wordLength selects word length, bits, [4; 16] or
	 * [ChipSpiMasterLowLevel::minWordLength; ChipSpiMasterLowLevel::maxWordLength]
	 *
	 * \return current value of DR register
	 */

	uint32_t readDr(const uint8_t wordLength) const
	{
		return wordLength <= 8 ? *reinterpret_cast<volatile uint8_t*>(&getSpi().DR) : getSpi().DR;
	}

	/**
	 * \return current value of SR register
	 */

	uint32_t readSr() const
	{
		return getSpi().SR;
	}

	/**
	 * \brief Writes value to CR1 register.
	 *
	 * \param [in] cr1 is the value that will be written to CR1 register
	 */

	void writeCr1(const uint32_t cr1) const
	{
		getSpi().CR1 = cr1;
	}

	/**
	 * \brief Writes value to CR2 register.
	 *
	 * \param [in] cr2 is the value that will be written to CR2 register
	 */

	void writeCr2(const uint32_t cr2) const
	{
		getSpi().CR2 = cr2;
	}

	/**
	 * \brief Writes value to DR register.
	 *
	 * \param [in] wordLength selects word length, bits, [4; 16] or
	 * [ChipSpiMasterLowLevel::minWordLength; ChipSpiMasterLowLevel::maxWordLength]
	 * \param [in] dr is the value that will be written to DR register
	 */

	void writeDr(const uint8_t wordLength, const uint32_t dr) const
	{
		if (wordLength <= 8)
			*reinterpret_cast<volatile uint8_t*>(&getSpi().DR) = dr;
		else
			getSpi().DR = dr;
	}

	/**
	 * \brief Writes value to SR register.
	 *
	 * \param [in] sr is the value that will be written to SR register
	 */

	void writeSr(const uint32_t sr) const
	{
		getSpi().SR = sr;
	}

private:

	/**
	 * \return reference to SPI_TypeDef object
	 */

	SPI_TypeDef& getSpi() const
	{
		return *reinterpret_cast<SPI_TypeDef*>(spiBase_);
	}

	/// base address of SPI peripheral
	uintptr_t spiBase_;

	/// peripheral clock frequency, Hz
	uint32_t peripheralFrequency_;
};

}	// namespace chip

}	// namespace distortos

#endif	// SOURCE_CHIP_STM32_PERIPHERALS_SPIV2_INCLUDE_DISTORTOS_CHIP_STM32_SPIV2_SPIPERIPHERAL_HPP_
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: enumhelper.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 02:45:10 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _COMPHELPER_ENUMHELPER_HXX_
#include <comphelper/enumhelper.hxx>
#endif

#ifndef _COM_SUN_STAR_LANG_XCOMPONENT_HPP_
#include <com/sun/star/lang/XComponent.hpp>
#endif

//.........................................................................
namespace comphelper
{
//.........................................................................

//==================================================================
//= OEnumerationByName
//==================================================================
//------------------------------------------------------------------------------
OEnumerationByName::OEnumerationByName(const staruno::Reference<starcontainer::XNameAccess>& _rxAccess)
    :m_aNames(_rxAccess->getElementNames())
    ,m_nPos(0)
    ,m_xAccess(_rxAccess)
    ,m_bListening(sal_False)
{
    impl_startDisposeListening();
}

//------------------------------------------------------------------------------
OEnumerationByName::OEnumerationByName(const staruno::Reference<starcontainer::XNameAccess>& _rxAccess,
                                       const staruno::Sequence< ::rtl::OUString >&           _aNames  )
    :m_aNames(_aNames)
    ,m_nPos(0)
    ,m_xAccess(_rxAccess)
    ,m_bListening(sal_False)
{
    impl_startDisposeListening();
}

//------------------------------------------------------------------------------
OEnumerationByName::~OEnumerationByName()
{
    impl_stopDisposeListening();
}

//------------------------------------------------------------------------------
sal_Bool SAL_CALL OEnumerationByName::hasMoreElements(  ) throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_xAccess.is() && m_aNames.getLength() > m_nPos)
        return sal_True;

    if (m_xAccess.is())
    {
        impl_stopDisposeListening();
        m_xAccess.clear();
    }

    return sal_False;
}

//------------------------------------------------------------------------------
staruno::Any SAL_CALL OEnumerationByName::nextElement(  )
        throw(starcontainer::NoSuchElementException, starlang::WrappedTargetException, staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    staruno::Any aRes;
    if (m_xAccess.is() && m_nPos < m_aNames.getLength())
        aRes = m_xAccess->getByName(m_aNames.getConstArray()[m_nPos++]);

    if (m_xAccess.is() && m_nPos >= m_aNames.getLength())
    {
        impl_stopDisposeListening();
        m_xAccess.clear();
    }

    if (!aRes.hasValue())       // es gibt kein Element mehr
        throw starcontainer::NoSuchElementException();

    return aRes;
}

//------------------------------------------------------------------------------
void SAL_CALL OEnumerationByName::disposing(const starlang::EventObject& aEvent)
        throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (aEvent.Source == m_xAccess)
        m_xAccess.clear();
}

//------------------------------------------------------------------------------
void OEnumerationByName::impl_startDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->addEventListener(this);
        m_bListening = sal_True;
    }
    --m_refCount;
}

//------------------------------------------------------------------------------
void OEnumerationByName::impl_stopDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (!m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->removeEventListener(this);
        m_bListening = sal_False;
    }
    --m_refCount;
}

//==================================================================
//= OEnumerationByIndex
//==================================================================
//------------------------------------------------------------------------------
OEnumerationByIndex::OEnumerationByIndex(const staruno::Reference< starcontainer::XIndexAccess >& _rxAccess)
    :m_xAccess(_rxAccess)
    ,m_nPos(0)
    ,m_bListening(sal_False)
{
    impl_startDisposeListening();
}

//------------------------------------------------------------------------------
OEnumerationByIndex::~OEnumerationByIndex()
{
    impl_stopDisposeListening();
}

//------------------------------------------------------------------------------
sal_Bool SAL_CALL OEnumerationByIndex::hasMoreElements(  ) throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_xAccess.is() && m_xAccess->getCount() > m_nPos)
        return sal_True;

    if (m_xAccess.is())
    {
        impl_stopDisposeListening();
        m_xAccess.clear();
    }

    return sal_False;
}

//------------------------------------------------------------------------------
staruno::Any SAL_CALL OEnumerationByIndex::nextElement(  )
        throw(starcontainer::NoSuchElementException, starlang::WrappedTargetException, staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    staruno::Any aRes;
    if (m_xAccess.is())
    {
        aRes = m_xAccess->getByIndex(m_nPos++);
        if (m_nPos >= m_xAccess->getCount())
        {
            impl_stopDisposeListening();
            m_xAccess.clear();
        }
    }

    if (!aRes.hasValue())       // es gibt kein Element mehr
        throw starcontainer::NoSuchElementException();
    return aRes;
}

//------------------------------------------------------------------------------
void SAL_CALL OEnumerationByIndex::disposing(const starlang::EventObject& aEvent)
        throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (aEvent.Source == m_xAccess)
        m_xAccess.clear();
}

//------------------------------------------------------------------------------
void OEnumerationByIndex::impl_startDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->addEventListener(this);
        m_bListening = sal_True;
    }
    --m_refCount;
}

//------------------------------------------------------------------------------
void OEnumerationByIndex::impl_stopDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (!m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->removeEventListener(this);
        m_bListening = sal_False;
    }
    --m_refCount;
}

//.........................................................................
}   // namespace comphelper
//.........................................................................

<commit_msg>INTEGRATION: CWS warnings01 (1.4.184); FILE MERGED 2005/09/23 03:13:48 sb 1.4.184.2: RESYNC: (1.4-1.5); FILE MERGED 2005/09/01 13:59:56 sb 1.4.184.1: #i53898# Made code warning-free.<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: enumhelper.cxx,v $
 *
 *  $Revision: 1.6 $
 *
 *  last change: $Author: hr $ $Date: 2006-06-19 22:46:20 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _COMPHELPER_ENUMHELPER_HXX_
#include <comphelper/enumhelper.hxx>
#endif

#ifndef _COM_SUN_STAR_LANG_XCOMPONENT_HPP_
#include <com/sun/star/lang/XComponent.hpp>
#endif

//.........................................................................
namespace comphelper
{
//.........................................................................

//==================================================================
//= OEnumerationByName
//==================================================================
//------------------------------------------------------------------------------
OEnumerationByName::OEnumerationByName(const staruno::Reference<starcontainer::XNameAccess>& _rxAccess)
    :m_aNames(_rxAccess->getElementNames())
    ,m_nPos(0)
    ,m_xAccess(_rxAccess)
    ,m_bListening(sal_False)
{
    impl_startDisposeListening();
}

//------------------------------------------------------------------------------
OEnumerationByName::OEnumerationByName(const staruno::Reference<starcontainer::XNameAccess>& _rxAccess,
                                       const staruno::Sequence< ::rtl::OUString >&           _aNames  )
    :m_aNames(_aNames)
    ,m_nPos(0)
    ,m_xAccess(_rxAccess)
    ,m_bListening(sal_False)
{
    impl_startDisposeListening();
}

//------------------------------------------------------------------------------
OEnumerationByName::~OEnumerationByName()
{
    impl_stopDisposeListening();
}

//------------------------------------------------------------------------------
sal_Bool SAL_CALL OEnumerationByName::hasMoreElements(  ) throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_xAccess.is() && m_aNames.getLength() > m_nPos)
        return sal_True;

    if (m_xAccess.is())
    {
        impl_stopDisposeListening();
        m_xAccess.clear();
    }

    return sal_False;
}

//------------------------------------------------------------------------------
staruno::Any SAL_CALL OEnumerationByName::nextElement(  )
        throw(starcontainer::NoSuchElementException, starlang::WrappedTargetException, staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    staruno::Any aRes;
    if (m_xAccess.is() && m_nPos < m_aNames.getLength())
        aRes = m_xAccess->getByName(m_aNames.getConstArray()[m_nPos++]);

    if (m_xAccess.is() && m_nPos >= m_aNames.getLength())
    {
        impl_stopDisposeListening();
        m_xAccess.clear();
    }

    if (!aRes.hasValue())       // es gibt kein Element mehr
        throw starcontainer::NoSuchElementException();

    return aRes;
}

//------------------------------------------------------------------------------
void SAL_CALL OEnumerationByName::disposing(const starlang::EventObject& aEvent)
        throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (aEvent.Source == m_xAccess)
        m_xAccess.clear();
}

//------------------------------------------------------------------------------
void OEnumerationByName::impl_startDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->addEventListener(this);
        m_bListening = sal_True;
    }
    --m_refCount;
}

//------------------------------------------------------------------------------
void OEnumerationByName::impl_stopDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (!m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->removeEventListener(this);
        m_bListening = sal_False;
    }
    --m_refCount;
}

//==================================================================
//= OEnumerationByIndex
//==================================================================
//------------------------------------------------------------------------------
OEnumerationByIndex::OEnumerationByIndex(const staruno::Reference< starcontainer::XIndexAccess >& _rxAccess)
    :m_nPos(0)
    ,m_xAccess(_rxAccess)
    ,m_bListening(sal_False)
{
    impl_startDisposeListening();
}

//------------------------------------------------------------------------------
OEnumerationByIndex::~OEnumerationByIndex()
{
    impl_stopDisposeListening();
}

//------------------------------------------------------------------------------
sal_Bool SAL_CALL OEnumerationByIndex::hasMoreElements(  ) throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_xAccess.is() && m_xAccess->getCount() > m_nPos)
        return sal_True;

    if (m_xAccess.is())
    {
        impl_stopDisposeListening();
        m_xAccess.clear();
    }

    return sal_False;
}

//------------------------------------------------------------------------------
staruno::Any SAL_CALL OEnumerationByIndex::nextElement(  )
        throw(starcontainer::NoSuchElementException, starlang::WrappedTargetException, staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    staruno::Any aRes;
    if (m_xAccess.is())
    {
        aRes = m_xAccess->getByIndex(m_nPos++);
        if (m_nPos >= m_xAccess->getCount())
        {
            impl_stopDisposeListening();
            m_xAccess.clear();
        }
    }

    if (!aRes.hasValue())       // es gibt kein Element mehr
        throw starcontainer::NoSuchElementException();
    return aRes;
}

//------------------------------------------------------------------------------
void SAL_CALL OEnumerationByIndex::disposing(const starlang::EventObject& aEvent)
        throw(staruno::RuntimeException)
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (aEvent.Source == m_xAccess)
        m_xAccess.clear();
}

//------------------------------------------------------------------------------
void OEnumerationByIndex::impl_startDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->addEventListener(this);
        m_bListening = sal_True;
    }
    --m_refCount;
}

//------------------------------------------------------------------------------
void OEnumerationByIndex::impl_stopDisposeListening()
{
    ::osl::ResettableMutexGuard aLock(m_aLock);

    if (!m_bListening)
        return;

    ++m_refCount;
    staruno::Reference< starlang::XComponent > xDisposable(m_xAccess, staruno::UNO_QUERY);
    if (xDisposable.is())
    {
        xDisposable->removeEventListener(this);
        m_bListening = sal_False;
    }
    --m_refCount;
}

//.........................................................................
}   // namespace comphelper
//.........................................................................

<|endoftext|>


<commit_before>#include <iostream>

#define CATCH_CONFIG_RUNNER
#include "catch.hpp"

#include <sodium.h>

int main(int argc, char **argv)
{
    if (sodium_init() == -1) {
        std::cerr << "Failed to initialize libsodium" << std::endl;
        return 1;
    }

    Catch::Session session;
    int ret = session.applyCommandLine(argc, argv);
    if (ret)
        return ret;

    session.run();

    return 0;
}
<commit_msg>fix unit test program return<commit_after>#include <iostream>

#define CATCH_CONFIG_RUNNER
#include "catch.hpp"

#include <sodium.h>

int main(int argc, char **argv)
{
    if (sodium_init() == -1) {
        std::cerr << "Failed to initialize libsodium" << std::endl;
        return 1;
    }

    Catch::Session session;
    int ret = session.applyCommandLine(argc, argv);
    if (ret)
        return ret;

    ret = session.run();

    return ret;
}
<|endoftext|>


<commit_before>/**
 *  \file
 *  \remark This file is part of VITA.
 *
 *  \copyright Copyright (C) 2013-2017 EOS di Manlio Morini.
 *
 *  \license
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this file,
 *  You can obtain one at http://mozilla.org/MPL/2.0/
 */

#if !defined(VITA_EVOLUTION_STRATEGY_H)
#  error "Don't include this file directly, include the specific .h instead"
#endif

#if !defined(VITA_EVOLUTION_STRATEGY_TCC)
#define      VITA_EVOLUTION_STRATEGY_TCC

///
/// \param[out] env environment
/// \return         a strategy-specific environment
///
/// \remark For standard evolution we only need one layer.
///
template<class T>
environment std_es<T>::shape(environment env)
{
  env.layers = 1;
  return env;
}

///
/// \return `true` when evolution must be stopped
///
/// We use an accelerated stop condition when:
/// - all the individuals have the same fitness;
/// - after `env_.g_without_improvement` generations the situation doesn't
///   change.
///
template<class T>
bool std_es<T>::stop_condition() const
{
  const auto &env(this->pop_.env());

  if (env.g_without_improvement &&
      this->sum_->gen - this->sum_->last_imp > env.g_without_improvement &&
      issmall(this->sum_->az.fit_dist().variance()))
    return true;

  return false;
}

///
/// \param[out] env environemnt
/// \return         a strategy-specific environment
///
/// \remark ALPS requires more than one layer.
///
template<class T, template<class> class CS>
environment basic_alps_es<T, CS>::shape(environment env)
{
  env.layers = 4;
  return env;
}

///
/// Increments population's age and checks if it's time to add a new layer.
///
template<class T, template<class> class CS>
void basic_alps_es<T, CS>::post_bookkeeping()
{
  const auto &sum(this->sum_);
  auto &pop(this->pop_);

  pop.inc_age();

  const auto layers(pop.layers());
  for (auto l(decltype(layers){1}); l < layers; ++l)
  {
    const auto allowed(pop.env().individuals);
    const auto current(pop.individuals(l));
    if (issmall(sum->az.fit_dist(l).standard_deviation()))
    {
      if (current > allowed / 5)
        pop.set_allowed(l, current / 2);
    }
    else
      pop.set_allowed(l, allowed);
  }

  // Code executed every `age_gap` interval.
  if (sum->gen && sum->gen % pop.env().alps.age_gap == 0)
  {
    if (layers < pop.env().layers ||
        sum->az.age_dist(layers - 1).mean() >
        alps::max_age(layers, pop.env().alps.age_gap))
      pop.add_layer();
    else
    {
      this->replacement.try_move_up_layer(0);
      pop.init_layer(0);
    }
  }
}

///
/// Saves working / statistical informations about layer status.
///
/// \param[in] last_run    last run processed
/// \param[in] current_run current run
///
/// Parameters from the environment:
/// * env.stat.layers if `false` the method will not write any data.
///
template<class T, template<class> class CS>
void basic_alps_es<T, CS>::log(unsigned last_run, unsigned current_run) const
{
  const auto &pop(this->pop_);
  const auto &env(pop.env());

  if (env.stat.layers)
  {
    const std::string n_lys(env.stat.dir + "/" + env.stat.lys_name);
    std::ofstream f_lys(n_lys, std::ios_base::app);
    if (!f_lys.good())
      return;

    if (last_run != current_run)
      f_lys << "\n\n";

    auto layers(pop.layers());
    for (decltype(layers) l(0); l < layers; ++l)
    {
      f_lys << current_run << ' ' << this->sum_->gen << ' ' << l << " <";

      const auto ma(alps::allowed_age(pop, l));
      if (ma == std::numeric_limits<decltype(ma)>::max())
        f_lys << "inf";
      else
        f_lys << ma + 1;

      f_lys << ' ' << this->sum_->az.age_dist(l).mean()
            << ' ' << this->sum_->az.age_dist(l).standard_deviation()
            << ' ' << static_cast<unsigned>(this->sum_->az.age_dist(l).min())
            << '-' << static_cast<unsigned>(this->sum_->az.age_dist(l).max())
            << ' ' << this->sum_->az.fit_dist(l).mean()
            << ' ' << this->sum_->az.fit_dist(l).standard_deviation()
            << ' ' << this->sum_->az.fit_dist(l).min()
            << '-' << this->sum_->az.fit_dist(l).max()
            << ' ' << pop.individuals(l) << '\n';
    }
  }
}
#endif  // include guard
<commit_msg>[REF] ALPS can remove multiple indentical layers<commit_after>/**
 *  \file
 *  \remark This file is part of VITA.
 *
 *  \copyright Copyright (C) 2013-2017 EOS di Manlio Morini.
 *
 *  \license
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this file,
 *  You can obtain one at http://mozilla.org/MPL/2.0/
 */

#if !defined(VITA_EVOLUTION_STRATEGY_H)
#  error "Don't include this file directly, include the specific .h instead"
#endif

#if !defined(VITA_EVOLUTION_STRATEGY_TCC)
#define      VITA_EVOLUTION_STRATEGY_TCC

///
/// \param[out] env environment
/// \return         a strategy-specific environment
///
/// \remark For standard evolution we only need one layer.
///
template<class T>
environment std_es<T>::shape(environment env)
{
  env.layers = 1;
  return env;
}

///
/// \return `true` when evolution must be stopped
///
/// We use an accelerated stop condition when:
/// - all the individuals have the same fitness;
/// - after `env_.g_without_improvement` generations the situation doesn't
///   change.
///
template<class T>
bool std_es<T>::stop_condition() const
{
  const auto &env(this->pop_.env());

  if (env.g_without_improvement &&
      this->sum_->gen - this->sum_->last_imp > env.g_without_improvement &&
      issmall(this->sum_->az.fit_dist().variance()))
    return true;

  return false;
}

///
/// \param[out] env environemnt
/// \return         a strategy-specific environment
///
/// \remark ALPS requires more than one layer.
///
template<class T, template<class> class CS>
environment basic_alps_es<T, CS>::shape(environment env)
{
  env.layers = 4;
  return env;
}

///
/// Increments population's age and checks if it's time to add a new layer.
///
template<class T, template<class> class CS>
void basic_alps_es<T, CS>::post_bookkeeping()
{
  const auto &sum(this->sum_);
  auto &pop(this->pop_);

  pop.inc_age();

  for (auto l(pop.layers() - 1); l; --l)
    if (almost_equal(sum->az.fit_dist(l - 1).mean(),
                     sum->az.fit_dist(    l).mean()))
      pop.remove_layer(l);

  auto layers(pop.layers());

  for (decltype(layers) l(1); l < layers; ++l)
    if (issmall(sum->az.fit_dist(l).standard_deviation()))
    {
      const auto current(pop.individuals(l));
      pop.set_allowed(l, std::max(pop.env().min_individuals, current / 2));
    }
    else
    {
      const auto allowed(pop.env().individuals);
      pop.set_allowed(l, allowed);
    }

  // Code executed every `age_gap` interval.
  if (sum->gen && sum->gen % pop.env().alps.age_gap == 0)
  {
    if (layers < pop.env().layers ||
        sum->az.age_dist(layers - 1).mean() >
        alps::max_age(layers, pop.env().alps.age_gap))
      pop.add_layer();
    else
    {
      this->replacement.try_move_up_layer(0);
      pop.init_layer(0);
    }
  }
}

///
/// Saves working / statistical informations about layer status.
///
/// \param[in] last_run    last run processed
/// \param[in] current_run current run
///
/// Parameters from the environment:
/// * env.stat.layers if `false` the method will not write any data.
///
template<class T, template<class> class CS>
void basic_alps_es<T, CS>::log(unsigned last_run, unsigned current_run) const
{
  const auto &pop(this->pop_);
  const auto &env(pop.env());

  if (env.stat.layers)
  {
    const std::string n_lys(env.stat.dir + "/" + env.stat.lys_name);
    std::ofstream f_lys(n_lys, std::ios_base::app);
    if (!f_lys.good())
      return;

    if (last_run != current_run)
      f_lys << "\n\n";

    auto layers(pop.layers());
    for (decltype(layers) l(0); l < layers; ++l)
    {
      f_lys << current_run << ' ' << this->sum_->gen << ' ' << l << " <";

      const auto ma(alps::allowed_age(pop, l));
      if (ma == std::numeric_limits<decltype(ma)>::max())
        f_lys << "inf";
      else
        f_lys << ma + 1;

      f_lys << ' ' << this->sum_->az.age_dist(l).mean()
            << ' ' << this->sum_->az.age_dist(l).standard_deviation()
            << ' ' << static_cast<unsigned>(this->sum_->az.age_dist(l).min())
            << '-' << static_cast<unsigned>(this->sum_->az.age_dist(l).max())
            << ' ' << this->sum_->az.fit_dist(l).mean()
            << ' ' << this->sum_->az.fit_dist(l).standard_deviation()
            << ' ' << this->sum_->az.fit_dist(l).min()
            << '-' << this->sum_->az.fit_dist(l).max()
            << ' ' << pop.individuals(l) << '\n';
    }
  }
}
#endif  // include guard
<|endoftext|>


<commit_before>/*
 TGD2151 Computer Graphics Fundamentals
 Faculty of Computing & Informatics, Multimedia University

 CGLab01.hpp

 Objective: Header File for Lab01 Demo Models / World

 (C) 2006-2015 Ya Ping Wong, All Rights Reserved.
 http://wongyaping.com

 Stanford Dragon:
 Original Model : Copyright by Stanford University
 http://graphics.stanford.edu/data/3Dscanrep/
 Low-polygons Model : Simplified by Mr. Ng Kok Why
 <kwng@mmu.edu.my>

 INSTRUCTIONS
 ============
 Please refer to CGLabmain.cpp for instructions

 SPECIAL NOTES
 =============
 * Try loading the high-polygons version of
 the Dragon model, it will be slow in slower PC.
 * Try out other models from http://graphics.stanford.edu/data/3Dscanrep/
 However, you will need to modify the file to comform to the
 format that is being used in this program.

 CHANGE LOG
 ==========
 */

#ifndef YP_CGLAB01_HPP
#define YP_CGLAB01_HPP

#include "CGLabmain.hpp"
#include "utilities/Mesh.hpp"
#include "utilities/Extrusion.hpp"
#include "utilities/Loft.hpp"
#include <string>
#include <vector>

namespace CGLab01 {

    class SimplePolygon
    {
    public:
        void draw();
    };

    class SimpleTriangles
    {
    public:
        void draw();
    };

    class SimpleBox
    {
    public:
        void draw();
    };

    class SimpleTeapot
    {
    public:
        void draw();
    };

    class SimpleBouncingBall
    {
    public:
        SimpleBouncingBall();
        void draw();
        void tickTime(long int elapseTime);
    private:
        long int timetick;
        float vel0;
        float accel;
    };

    class MyModelLoader
    {
    public:
        MyModelLoader()
        {
        }
        ~MyModelLoader()
        {
        }
        //load a model and scale it
        void load(string filename, float scale = 1.0);
        void draw();
    private:
        vector<GLfloat> vertices;
        vector<int> faces;
        GLuint stanforddragon; //for generating display list
    };

    //------------------------------------
    //the main program will call methods from this class
    class MyVirtualWorld
    {
    public:
        SimplePolygon      simplepolygon;
        SimpleBox          simplebox;
        SimpleTriangles    simpletriangles;
        SimpleTeapot       simpleteapot;
        SimpleBouncingBall simplebouncingball;
        MyModelLoader      mymodelloader;
        Mesh *deer;
        Mesh *elephant;
        vector<vec2> points;
        Extrusion *extrude;

        Loft *loft;

        vector<vec3> pts, ptsTransformed,points3d;

        long int timeold, timenew, elapseTime;

        void draw();

        ~MyVirtualWorld() {
            delete deer;
            delete elephant;
            delete extrude;
        }

        void tickTime()
        {

            timenew    = glutGet(GLUT_ELAPSED_TIME);
            elapseTime = timenew - timeold;
            timeold    = timenew;

            simplebouncingball.tickTime(elapseTime);
        }

        //for any one-time only initialization of the
        //   virtual world before any rendering takes place
        //   BUT after OpenGL has been initialized
        void init()
        {
            glEnable(GL_LIGHTING);

            points = {
                {{ -4.0f, -5.0f }}, {{ -4.0f,  5.0f }},
                {{  0.0f,  7.0f }}, {{  4.0f,  5.0f }},
                {{  4.0f, -5.0f }}, {{  0.0f, -7.0f }}
            };

            points3d = {
                {{ -4.0f, -5.0f,5.0f }},
                {{  4.0f,  5.0f,5.0f }},
                {{  8.0f,  7.0f,5.0f }},
                {{  12.0f,  5.0f,5.0f }},
                {{  16.0f, -5.0f,5.0f }},
                {{  20.0f, -7.0f,5.0f }}
            };

            extrude = new Extrusion(points);
            extrude->setDepth(8);
            loft = new Loft(points, points3d);

            //Low-polygons dragon (5835 triangles)
            mymodelloader.load("data/model_lowpolygonstanforddragon.txt",100);
            deer = new Mesh("data/deer.obj");
            deer->setFlatColor({{.8, .2, .8}});
            deer->setTranslateX(10.5f);
            deer->setScale(0.5f);

            elephant = new Mesh("data/elephant-triangulated.obj");
            elephant->setFlatColor({{ .8, .1, .15 }});
            elephant->setTranslateX(-10.5f);
            elephant->setRotateY(-45.0f);

            pts = getCircle(8, 7);

            vec3 startNormal  = {{ 0, 1, 0 }};
            vec3 targetNormal = {{ 0.3333, 0.3333, 0.3333 }};
            mat3 rotationMatrix = getRotationMatrix(startNormal, targetNormal);

            for (auto &p : pts) {
                ptsTransformed.push_back(mult(rotationMatrix, p));
            }
            //Try this:
            //High-polygons dragon (original model of Stanford Dragon)
            // (871414 triangles) will take some minutes for it to get loaded
            //mymodelloader.load("data/model_highpolygonstanforddragon.txt",100);

            //mymodelloader.load("data/model_rose.txt", 0.2);

            //mymodelloader.load("data/model_shuttle.txt", 0.1);

            timeold = glutGet(GLUT_ELAPSED_TIME);

        }
    };

}; //end of namespace CGLab01

#endif //YP_CGLAB01_HPP
<commit_msg>spring, heart<commit_after>/*
 TGD2151 Computer Graphics Fundamentals
 Faculty of Computing & Informatics, Multimedia University

 CGLab01.hpp

 Objective: Header File for Lab01 Demo Models / World

 (C) 2006-2015 Ya Ping Wong, All Rights Reserved.
 http://wongyaping.com

 Stanford Dragon:
 Original Model : Copyright by Stanford University
 http://graphics.stanford.edu/data/3Dscanrep/
 Low-polygons Model : Simplified by Mr. Ng Kok Why
 <kwng@mmu.edu.my>

 INSTRUCTIONS
 ============
 Please refer to CGLabmain.cpp for instructions

 SPECIAL NOTES
 =============
 * Try loading the high-polygons version of
 the Dragon model, it will be slow in slower PC.
 * Try out other models from http://graphics.stanford.edu/data/3Dscanrep/
 However, you will need to modify the file to comform to the
 format that is being used in this program.

 CHANGE LOG
 ==========
 */

#ifndef YP_CGLAB01_HPP
#define YP_CGLAB01_HPP

#include "CGLabmain.hpp"
#include "utilities/Mesh.hpp"
#include "utilities/Extrusion.hpp"
#include "utilities/Loft.hpp"
#include <string>
#include <vector>

namespace CGLab01 {

    class SimplePolygon
    {
    public:
        void draw();
    };

    class SimpleTriangles
    {
    public:
        void draw();
    };

    class SimpleBox
    {
    public:
        void draw();
    };

    class SimpleTeapot
    {
    public:
        void draw();
    };

    class SimpleBouncingBall
    {
    public:
        SimpleBouncingBall();
        void draw();
        void tickTime(long int elapseTime);
    private:
        long int timetick;
        float vel0;
        float accel;
    };

    class MyModelLoader
    {
    public:
        MyModelLoader()
        {
        }
        ~MyModelLoader()
        {
        }
        //load a model and scale it
        void load(string filename, float scale = 1.0);
        void draw();
    private:
        vector<GLfloat> vertices;
        vector<int> faces;
        GLuint stanforddragon; //for generating display list
    };

    //------------------------------------
    //the main program will call methods from this class
    class MyVirtualWorld
    {
    public:
        SimplePolygon      simplepolygon;
        SimpleBox          simplebox;
        SimpleTriangles    simpletriangles;
        SimpleTeapot       simpleteapot;
        SimpleBouncingBall simplebouncingball;
        MyModelLoader      mymodelloader;
        Mesh *deer;
        Mesh *elephant;
        vector<vec2> points;
        Extrusion *extrude;

        Loft *loft;

        vector<vec3> pts, ptsTransformed,points3d;

        long int timeold, timenew, elapseTime;

        void draw();

        ~MyVirtualWorld() {
            delete deer;
            delete elephant;
            delete extrude;
        }

        void tickTime()
        {

            timenew    = glutGet(GLUT_ELAPSED_TIME);
            elapseTime = timenew - timeold;
            timeold    = timenew;

            simplebouncingball.tickTime(elapseTime);
        }

        //for any one-time only initialization of the
        //   virtual world before any rendering takes place
        //   BUT after OpenGL has been initialized
        void init()
        {
            glEnable(GL_LIGHTING);

/*
            points = {
                {{ -4.0f, -5.0f }}, {{ -4.0f,  5.0f }},
                {{  0.0f,  7.0f }}, {{  4.0f,  5.0f }},
                {{  4.0f, -5.0f }}, {{  0.0f, -7.0f }}
            };

*/
            points3d = {
                {{ -4.0f, -5.0f,5.0f }},
                {{  4.0f,  5.0f,5.0f }},
                {{  8.0f,  7.0f,5.0f }},
                {{  12.0f,  5.0f,5.0f }},
                {{  16.0f, -5.0f,5.0f }},
                {{  20.0f, -7.0f,5.0f }}
            };

            auto circle = getCircle(2, 10);
            for (auto &v : circle) points.push_back({{ v[0], v[2] }});

            //spring:
            points3d = generateSpline(-50, 50, 150,
                                      [](float z)->float { return sin(z/2.0) * 15; },
                                      [](float x)->float { return cos(x/2.0) * 15; },
                                      [](float y)->float { return y; });
            // heart:
            /*
            points3d = generateSpline(-50, 50, 150,
                                      [](float z)->float {
                                            float t = z/5.0;
                                            return 16 * sin(t) * sin(t) * sin(t);
                                        },
                                      [](float x)->float {
                                            float t = x/5.0;
                                            return 13 * cos(t) - 5*cos(2*t) - 2*cos(3*t) - cos(4*t);
                                        });
            */

            extrude = new Extrusion(points);
            extrude->setDepth(8);
            loft = new Loft(points, points3d);

            //Low-polygons dragon (5835 triangles)
            mymodelloader.load("data/model_lowpolygonstanforddragon.txt",100);
            deer = new Mesh("data/deer.obj");
            deer->setFlatColor({{.8, .2, .8}});
            deer->setTranslateX(10.5f);
            deer->setScale(0.5f);

            elephant = new Mesh("data/elephant-triangulated.obj");
            elephant->setFlatColor({{ .8, .1, .15 }});
            elephant->setTranslateX(-10.5f);
            elephant->setRotateY(-45.0f);

            pts = getCircle(8, 7);

            vec3 startNormal  = {{ 0, 1, 0 }};
            vec3 targetNormal = {{ 0.3333, 0.3333, 0.3333 }};
            mat3 rotationMatrix = getRotationMatrix(startNormal, targetNormal);

            for (auto &p : pts) {
                ptsTransformed.push_back(mult(rotationMatrix, p));
            }
            //Try this:
            //High-polygons dragon (original model of Stanford Dragon)
            // (871414 triangles) will take some minutes for it to get loaded
            //mymodelloader.load("data/model_highpolygonstanforddragon.txt",100);

            //mymodelloader.load("data/model_rose.txt", 0.2);

            //mymodelloader.load("data/model_shuttle.txt", 0.1);

            timeold = glutGet(GLUT_ELAPSED_TIME);

        }
    };

}; //end of namespace CGLab01

#endif //YP_CGLAB01_HPP
<|endoftext|>


<commit_before>// Copyright (c) 2015, Ming Wen

#include "fys.hpp"
#include "json_handler.hpp"

using namespace std;

namespace fys {

void testBasicIO(string featuresFile)
{
    std::cout << "==== Test Start: BasicIO ====" << std::endl;

    JsonFeatures jf = JsonFeatures(featuresFile);
    jf.readJsonFile();
    std::cout << jf.getFileStr() << std::endl;

    rapidjson::Document doc;
    doc.Parse(jf.getFileStr().c_str());

    if (doc.HasMember("detector") && doc["detector"].IsNull()) {
        rapidjson::Value& detectorVal = doc["detector"];
        detectorVal.SetString("SIFT");
    }

    rapidjson::StringBuffer buffer;
    rapidjson::PrettyWriter<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);

    jf.updateStr(buffer);
    std::cout << jf.getFileStr() << std::endl;
    jf.writeJsonFile();

    std::cout << "==== Test End: BasicIO ====" << std::endl;
}

void testGetSet(string imageFile)
{
    std::cout << "==== Test Start: Getter and Setter ====" << std::endl;

    JsonImages ji = JsonImages(imageFile);
    ji.readJsonFile();

    rapidjson::Document doc;
    doc.Parse(ji.getFileStr().c_str());

    // getIntVal
    vector<string> path1 = vector<string>();
    path1.push_back("train");
    path1.push_back("0");
    path1.push_back("size");
    path1.push_back("nrows");
    std::cout << "Number of rows in the image: " << ji.getIntVal(doc, path1) << std::endl;

    // getDoubleVal & setDoubleVal
    vector<string> path2 = vector<string>();
    path2.push_back("train");
    path2.push_back("0");
    path2.push_back("objects");
    path2.push_back("0");
    path2.push_back("region");
    path2.push_back("ymin");
    std::cout << "Original: min y value of the object: " << ji.getDoubleVal(doc, path2) << std::endl;
    ji.setDoubleVal(doc, path2, 10);
    std::cout << "New: min y value of the object: " << ji.getDoubleVal(doc, path2) << std::endl;

    // getStrVal & setStrVal
    vector<string> path3 = vector<string>();
    path3.push_back("train");
    path3.push_back("0");
    path3.push_back("filename");
    std::cout << "Original: file name of image: " << ji.getStrVal(doc, path3) << std::endl;
    ji.setStrVal(doc, path3, "ff.jpg");
    std::cout << "New: file name of image: " << ji.getStrVal(doc, path3) << std::endl;

    rapidjson::StringBuffer buffer;
    rapidjson::PrettyWriter<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);
    std::cout << "Json string after change: " << ji.getDocStr(buffer) << std::endl;

    std::cout << "==== Test End: Getter and Setter ====" << std::endl;
}

void testFeatureType(string featuresFile)
{
    std::cout << "==== Test Start: Feature Types ====" << std::endl;

    JsonFeatures jf = JsonFeatures(featuresFile);
    jf.readJsonFile();

    rapidjson::Document doc;
    doc.Parse(jf.getFileStr().c_str());

    std::cout << "Original: detector type: " << jf.getDetectorType(doc) << std::endl;
    jf.setDetectorType(doc, "SURF");
    std::cout << "New: detector type: " << jf.getDetectorType(doc) << std::endl;

    rapidjson::StringBuffer buffer;
    rapidjson::PrettyWriter<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);
    std::cout << "Json string after change: " << jf.getDocStr(buffer) << std::endl;

    std::cout << "==== Test End: Feature Types ====" << std::endl;
}

void testImageProperties(string imagesFile)
{
    std::cout << "==== Test Start: Number of Images ====" << std::endl;

    JsonImages ji = JsonImages(imagesFile);
    ji.readJsonFile();

    rapidjson::Document doc;
    doc.Parse(ji.getFileStr().c_str());

    ImageSample stat = ji.getNumImage(doc);
    std::cout << "Training set: " << stat.train << std::endl;
    std::cout << "Validation set: " << stat.validate << std::endl;
    std::cout << "Test set: " << stat.test << std::endl;
    std::cout << "Total: " << stat.total << std::endl;
    std::cout << std::endl;
    std::cout << "File name of image 0: " << ji.getFileName(doc, TRAIN_TYPE, 0) << std::endl;
    std::cout << "Folder name of image 0: " << ji.getFolderName(doc, TRAIN_TYPE, 0) << std::endl;

    ImageSize is = ji.getImageSize(doc, TRAIN_TYPE, 0);
    std::cout << "Rows: " << is.nrows << std::endl;
    std::cout << "Columns: " << is.ncols << std::endl;

    std::cout << "==== Test End: Number of Images ====" << std::endl;
}

} // namespace fys

<commit_msg>tested: getObjectList()<commit_after>// Copyright (c) 2015, Ming Wen

#include "fys.hpp"
#include "json_handler.hpp"

using namespace std;

namespace fys {

void testBasicIO(string featuresFile)
{
    std::cout << "==== Test Start: BasicIO ====" << std::endl;

    JsonFeatures jf = JsonFeatures(featuresFile);
    jf.readJsonFile();
    std::cout << jf.getFileStr() << std::endl;

    rapidjson::Document doc;
    doc.Parse(jf.getFileStr().c_str());

    if (doc.HasMember("detector") && doc["detector"].IsNull()) {
        rapidjson::Value& detectorVal = doc["detector"];
        detectorVal.SetString("SIFT");
    }

    rapidjson::StringBuffer buffer;
    rapidjson::PrettyWriter<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);

    jf.updateStr(buffer);
    std::cout << jf.getFileStr() << std::endl;
    jf.writeJsonFile();

    std::cout << "==== Test End: BasicIO ====" << std::endl;
}

void testGetSet(string imageFile)
{
    std::cout << "==== Test Start: Getter and Setter ====" << std::endl;

    JsonImages ji = JsonImages(imageFile);
    ji.readJsonFile();

    rapidjson::Document doc;
    doc.Parse(ji.getFileStr().c_str());

    // getIntVal
    vector<string> path1 = vector<string>();
    path1.push_back("train");
    path1.push_back("0");
    path1.push_back("size");
    path1.push_back("nrows");
    std::cout << "Number of rows in the image: " << ji.getIntVal(doc, path1) << std::endl;

    // getDoubleVal & setDoubleVal
    vector<string> path2 = vector<string>();
    path2.push_back("train");
    path2.push_back("0");
    path2.push_back("objects");
    path2.push_back("0");
    path2.push_back("region");
    path2.push_back("ymin");
    std::cout << "Original: min y value of the object: " << ji.getDoubleVal(doc, path2) << std::endl;
    ji.setDoubleVal(doc, path2, 10);
    std::cout << "New: min y value of the object: " << ji.getDoubleVal(doc, path2) << std::endl;

    // getStrVal & setStrVal
    vector<string> path3 = vector<string>();
    path3.push_back("train");
    path3.push_back("0");
    path3.push_back("filename");
    std::cout << "Original: file name of image: " << ji.getStrVal(doc, path3) << std::endl;
    ji.setStrVal(doc, path3, "ff.jpg");
    std::cout << "New: file name of image: " << ji.getStrVal(doc, path3) << std::endl;

    rapidjson::StringBuffer buffer;
    rapidjson::PrettyWriter<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);
    std::cout << "Json string after change: " << ji.getDocStr(buffer) << std::endl;

    std::cout << "==== Test End: Getter and Setter ====" << std::endl;
}

void testFeatureType(string featuresFile)
{
    std::cout << "==== Test Start: Feature Types ====" << std::endl;

    JsonFeatures jf = JsonFeatures(featuresFile);
    jf.readJsonFile();

    rapidjson::Document doc;
    doc.Parse(jf.getFileStr().c_str());

    std::cout << "Original: detector type: " << jf.getDetectorType(doc) << std::endl;
    jf.setDetectorType(doc, "SURF");
    std::cout << "New: detector type: " << jf.getDetectorType(doc) << std::endl;

    rapidjson::StringBuffer buffer;
    rapidjson::PrettyWriter<rapidjson::StringBuffer> writer(buffer);
    doc.Accept(writer);
    std::cout << "Json string after change: " << jf.getDocStr(buffer) << std::endl;

    std::cout << "==== Test End: Feature Types ====" << std::endl;
}

void testImageProperties(string imagesFile)
{
    std::cout << "==== Test Start: Number of Images ====" << std::endl;

    JsonImages ji = JsonImages(imagesFile);
    ji.readJsonFile();

    rapidjson::Document doc;
    doc.Parse(ji.getFileStr().c_str());

    ImageSample stat = ji.getNumImage(doc);
    std::cout << "Training set: " << stat.train << std::endl;
    std::cout << "Validation set: " << stat.validate << std::endl;
    std::cout << "Test set: " << stat.test << std::endl;
    std::cout << "Total: " << stat.total << std::endl;
    std::cout << std::endl;
    std::cout << "File name of image 0: " << ji.getFileName(doc, TRAIN_TYPE, 0) << std::endl;
    std::cout << "Folder name of image 0: " << ji.getFolderName(doc, TRAIN_TYPE, 0) << std::endl;

    ImageSize is = ji.getImageSize(doc, TRAIN_TYPE, 0);
    std::cout << "Rows: " << is.nrows << std::endl;
    std::cout << "Columns: " << is.ncols << std::endl;

    vector<ImageObject> oblist = ji.getObjectList(doc, TRAIN_TYPE, 0);
    std::cout << "Object name: " << oblist[0].name << std::endl;
    std::cout << "Object ID: " << oblist[0].id << std::endl;
    std::cout << "Object region: " << oblist[0].region.xmin << " " << oblist[0].region.xmax << " " \
        << oblist[0].region.ymin << " " << oblist[0].region.ymax << std::endl;

    std::cout << "==== Test End: Number of Images ====" << std::endl;
}

} // namespace fys

<|endoftext|>


<commit_before>#include "stdafx.h"
#include "cpu.h"
#include "cpu_instructions.h"
#include "mmu.h"
#include "ram.h"
#include "cartrige.h"
#include "interrupts.h"
#include "gpu.h"
#include "timer.h"
#include "joypad.h"
#include "apu.h"
#include "debugger.h"

struct TestReader final : public IMemory
{
	u8 a;

	public:
		TestReader() : a() {}

		u8 read_byte(u16 adress, u32 unused) override
		{
			return 0;
			//return 0xFF;
		}

		void write_byte(u16 adress, u8 val, u32 unused) override
		{
			if (adress == 0xFF01)
				a = val;

			if (adress == 0xFF02 && val == 0x81)
				printf("%c", a);
		}
};

struct SpeedSwitch : public IMemory
{
	bool double_speed = false;
	bool switch_speed = false;

	u8 read_byte(u16 adress, u32 unused) override
	{
		if (adress == 0xFF4D)
		{
			auto ret = change_bit(0xFF, double_speed, 7);
			return change_bit(ret, switch_speed, 0);
		}

		else
			return 0xFF;
	}

	void write_byte(u16 adress, u8 val, u32 key) override
	{
		if (adress == 0xFF4D)
		{
			if (key == 0xFFFFFFFF && val == 0xFF)
			{
				switch_speed = false;
				double_speed = !double_speed;
			}

			else
				switch_speed = check_bit(val, 0);
		}

		//else ignore
	}
};

int main(int argc, char *argv[])
{
	Interrupts ints;
	Timer timer(ints);
	MMU mmu;
	Ram ram;
	Cartrige cart;
	TestReader tr; //testing
	Joypad joypad;
	CPU cpu(mmu);
	Gpu gpu(ints);
	APU apu;
	SpeedSwitch speed;

	Debugger debugger;
	debugger.attach_mmu(make_function(&MMU::read_byte, &mmu), make_function(&MMU::write_byte, &mmu));
	debugger.attach_gpu(gpu.get_debug_func());

	cpu.attach_debugger(debugger.get_cpu());
	mmu.attach_debug_callback(make_function(&Debugger::check_memory_access, &debugger));

	SDL_Window* window = SDL_CreateWindow("Test",
											SDL_WINDOWPOS_UNDEFINED,
											SDL_WINDOWPOS_UNDEFINED,
											160 * 3,
											144 * 3,
											0);

	SDL_Renderer* rend = SDL_CreateRenderer(window, -1, 0);
	SDL_Texture* tex = SDL_CreateTexture(rend, SDL_PIXELFORMAT_ARGB8888, SDL_TEXTUREACCESS_STREAMING, 160, 144);
	SDL_Event ev;

	std::string file_name;

	while (true)
	{
		std::cout << "Insert cartrige path:\n";
		std::cin >> file_name;

		if (cart.load_cartrige(file_name))
		{
			std::cout << "Cartrige loaded!\n";
			break;
		}

		std::cout << "Failed to load cartrige!\n";
	}

	mmu.register_chunk(0, 0x7FFF, cart.get_memory_controller());
	mmu.register_chunk(0x8000, 0x9FFF, &gpu); //vram
	mmu.register_chunk(0xA000, 0xBFFF, cart.get_memory_controller());
	mmu.register_chunk(0xC000, 0xFDFF, &ram);
	mmu.register_chunk(0xFE00, 0xFE9F, &gpu); //oam tables
	mmu.register_chunk(0xFF00, 0xFF00, &joypad);//input keys register
	mmu.register_chunk(0xFF01, 0xFF02, &tr); //TEST READER!!!!
	mmu.register_chunk(0xFF04, 0xFF07, &timer);//timer controls
	mmu.register_chunk(0xFF0F, 0xFF0F, &ints);//interrupts flags

	mmu.register_chunk(0xFF10, 0xFF3F, &apu); //APU registers + wave RAM

	mmu.register_chunk(0xFF40, 0xFF4B, &gpu); //gpu control regs
	mmu.register_chunk(0xFF4D, 0xFF4D, &speed); //CPU speed switch (CGB)
	mmu.register_chunk(0xFF4F, 0xFF4F, &gpu); //gpu vram bank reg (CGB)
	mmu.register_chunk(0xFF51, 0xFF55, &gpu); //gpu HDMA/GDMA regs (CGB)
	mmu.register_chunk(0xFF68, 0xFF6B, &gpu); //gpu color BGP/OBP regs (CGB)
	mmu.register_chunk(0xFF70, 0xFF70, &ram); //ram bank register (CGB)
	mmu.register_chunk(0xFF80, 0xFFFE, &ram); //high ram
	mmu.register_chunk(0xFFFF, 0xFFFF, &ints); //interrupts

	gpu.attach_dma_ptrs(cart.get_dma_controller(), &ram);

	bool enable_cgb = cart.is_cgb_ready();
	cpu.enable_cgb_mode(enable_cgb);
	gpu.enable_cgb_mode(enable_cgb);
	ram.enable_cgb_mode(enable_cgb);

	bool spin = true;
	const u32 key_map[8] = { SDLK_RIGHT, SDLK_LEFT, SDLK_UP, SDLK_DOWN, SDLK_a, SDLK_b, SDLK_RETURN, SDLK_s };

	while (spin)
	{
		while (SDL_PollEvent(&ev))
		{
			if (ev.type == SDL_KEYDOWN || ev.type == SDL_KEYUP)
			{
				u32 key_code = 0;

				while (key_code < 8 && key_map[key_code] != ev.key.keysym.sym)
					++key_code;

				if (key_code < 8)
				{
					if (ev.type == SDL_KEYDOWN)
						joypad.push_key(static_cast<KEYS>(key_code));

					else
						joypad.release_key(static_cast<KEYS>(key_code));
				}
			}

			else if (ev.type == SDL_QUIT)
				spin = false;
		}

		//auto start = std::chrono::high_resolution_clock::now();
		while (!gpu.is_entering_vblank()) //TODO: if someone turn off lcd, this loop may spin forever
		{
			u32 sync_cycles = 0;

			if (ints.is_any_raised())
			{
				cpu.unhalt();

				if (cpu.is_interrupt_enabled())
					sync_cycles = cpu.handle_interrupt(ints.get_first_raised());
			}

			debugger.step();

			sync_cycles += cpu.step();
			sync_cycles += gpu.step(sync_cycles);
			apu.step(sync_cycles);
			timer.step(sync_cycles);

			gpu.set_speed(speed.double_speed);
			apu.set_speed(speed.double_speed);
		}

		auto ptr = gpu.get_frame_buffer();
		void* pixels = nullptr;
		int pitch = 0;

		SDL_LockTexture(tex, NULL, &pixels, &pitch);
		std::memcpy(pixels, ptr, sizeof(u32) * 160 * 144);
		SDL_UnlockTexture(tex);

		SDL_RenderClear(rend);
		SDL_RenderCopy(rend, tex, NULL, NULL);
		SDL_RenderPresent(rend);

		gpu.clear_frame_buffer();
		debugger.after_vblank();

		//auto end = std::chrono::high_resolution_clock::now();
		//auto dur = (end - start).count();

		/*if ((dur / 1000000) < 16)
			SDL_Delay(16 - (dur / 1000000));*/
	}

	SDL_DestroyTexture(tex);
	SDL_DestroyRenderer(rend);
	SDL_DestroyWindow(window);

	return 0;
}
<commit_msg>forgot to init SDL2<commit_after>#include "stdafx.h"
#include "cpu.h"
#include "cpu_instructions.h"
#include "mmu.h"
#include "ram.h"
#include "cartrige.h"
#include "interrupts.h"
#include "gpu.h"
#include "timer.h"
#include "joypad.h"
#include "apu.h"
#include "debugger.h"

struct TestReader final : public IMemory
{
	u8 a;

	public:
		TestReader() : a() {}

		u8 read_byte(u16 adress, u32 unused) override
		{
			return 0;
			//return 0xFF;
		}

		void write_byte(u16 adress, u8 val, u32 unused) override
		{
			if (adress == 0xFF01)
				a = val;

			if (adress == 0xFF02 && val == 0x81)
				printf("%c", a);
		}
};

struct SpeedSwitch : public IMemory
{
	bool double_speed = false;
	bool switch_speed = false;

	u8 read_byte(u16 adress, u32 unused) override
	{
		if (adress == 0xFF4D)
		{
			auto ret = change_bit(0xFF, double_speed, 7);
			return change_bit(ret, switch_speed, 0);
		}

		else
			return 0xFF;
	}

	void write_byte(u16 adress, u8 val, u32 key) override
	{
		if (adress == 0xFF4D)
		{
			if (key == 0xFFFFFFFF && val == 0xFF)
			{
				switch_speed = false;
				double_speed = !double_speed;
			}

			else
				switch_speed = check_bit(val, 0);
		}

		//else ignore
	}
};

int main(int argc, char *argv[])
{
	SDL_Init(SDL_INIT_VIDEO);

	Interrupts ints;
	Timer timer(ints);
	MMU mmu;
	Ram ram;
	Cartrige cart;
	TestReader tr; //testing
	Joypad joypad;
	CPU cpu(mmu);
	Gpu gpu(ints);
	APU apu;
	SpeedSwitch speed;

	Debugger debugger;
	debugger.attach_mmu(make_function(&MMU::read_byte, &mmu), make_function(&MMU::write_byte, &mmu));
	debugger.attach_gpu(gpu.get_debug_func());

	cpu.attach_debugger(debugger.get_cpu());
	mmu.attach_debug_callback(make_function(&Debugger::check_memory_access, &debugger));

	SDL_Window* window = SDL_CreateWindow("Test",
											SDL_WINDOWPOS_UNDEFINED,
											SDL_WINDOWPOS_UNDEFINED,
											160 * 3,
											144 * 3,
											0);

	SDL_Renderer* rend = SDL_CreateRenderer(window, -1, 0);
	SDL_Texture* tex = SDL_CreateTexture(rend, SDL_PIXELFORMAT_ARGB8888, SDL_TEXTUREACCESS_STREAMING, 160, 144);
	SDL_Event ev;

	std::string file_name;

	while (true)
	{
		std::cout << "Insert cartrige path:\n";
		std::cin >> file_name;

		if (cart.load_cartrige(file_name))
		{
			std::cout << "Cartrige loaded!\n";
			break;
		}

		std::cout << "Failed to load cartrige!\n";
	}

	mmu.register_chunk(0, 0x7FFF, cart.get_memory_controller());
	mmu.register_chunk(0x8000, 0x9FFF, &gpu); //vram
	mmu.register_chunk(0xA000, 0xBFFF, cart.get_memory_controller());
	mmu.register_chunk(0xC000, 0xFDFF, &ram);
	mmu.register_chunk(0xFE00, 0xFE9F, &gpu); //oam tables
	mmu.register_chunk(0xFF00, 0xFF00, &joypad);//input keys register
	mmu.register_chunk(0xFF01, 0xFF02, &tr); //TEST READER!!!!
	mmu.register_chunk(0xFF04, 0xFF07, &timer);//timer controls
	mmu.register_chunk(0xFF0F, 0xFF0F, &ints);//interrupts flags

	mmu.register_chunk(0xFF10, 0xFF3F, &apu); //APU registers + wave RAM

	mmu.register_chunk(0xFF40, 0xFF4B, &gpu); //gpu control regs
	mmu.register_chunk(0xFF4D, 0xFF4D, &speed); //CPU speed switch (CGB)
	mmu.register_chunk(0xFF4F, 0xFF4F, &gpu); //gpu vram bank reg (CGB)
	mmu.register_chunk(0xFF51, 0xFF55, &gpu); //gpu HDMA/GDMA regs (CGB)
	mmu.register_chunk(0xFF68, 0xFF6B, &gpu); //gpu color BGP/OBP regs (CGB)
	mmu.register_chunk(0xFF70, 0xFF70, &ram); //ram bank register (CGB)
	mmu.register_chunk(0xFF80, 0xFFFE, &ram); //high ram
	mmu.register_chunk(0xFFFF, 0xFFFF, &ints); //interrupts

	gpu.attach_dma_ptrs(cart.get_dma_controller(), &ram);

	bool enable_cgb = cart.is_cgb_ready();
	cpu.enable_cgb_mode(enable_cgb);
	gpu.enable_cgb_mode(enable_cgb);
	ram.enable_cgb_mode(enable_cgb);

	bool spin = true;
	const u32 key_map[8] = { SDLK_RIGHT, SDLK_LEFT, SDLK_UP, SDLK_DOWN, SDLK_a, SDLK_b, SDLK_RETURN, SDLK_s };

	while (spin)
	{
		while (SDL_PollEvent(&ev))
		{
			if (ev.type == SDL_KEYDOWN || ev.type == SDL_KEYUP)
			{
				u32 key_code = 0;

				while (key_code < 8 && key_map[key_code] != ev.key.keysym.sym)
					++key_code;

				if (key_code < 8)
				{
					if (ev.type == SDL_KEYDOWN)
						joypad.push_key(static_cast<KEYS>(key_code));

					else
						joypad.release_key(static_cast<KEYS>(key_code));
				}
			}

			else if (ev.type == SDL_QUIT)
				spin = false;
		}

		//auto start = std::chrono::high_resolution_clock::now();
		while (!gpu.is_entering_vblank()) //TODO: if someone turn off lcd, this loop may spin forever
		{
			u32 sync_cycles = 0;

			if (ints.is_any_raised())
			{
				cpu.unhalt();

				if (cpu.is_interrupt_enabled())
					sync_cycles = cpu.handle_interrupt(ints.get_first_raised());
			}

			debugger.step();

			sync_cycles += cpu.step();
			sync_cycles += gpu.step(sync_cycles);
			apu.step(sync_cycles);
			timer.step(sync_cycles);

			gpu.set_speed(speed.double_speed);
			apu.set_speed(speed.double_speed);
		}

		auto ptr = gpu.get_frame_buffer();
		void* pixels = nullptr;
		int pitch = 0;

		SDL_LockTexture(tex, NULL, &pixels, &pitch);
		std::memcpy(pixels, ptr, sizeof(u32) * 160 * 144);
		SDL_UnlockTexture(tex);

		SDL_RenderClear(rend);
		SDL_RenderCopy(rend, tex, NULL, NULL);
		SDL_RenderPresent(rend);

		gpu.clear_frame_buffer();
		debugger.after_vblank();

		//auto end = std::chrono::high_resolution_clock::now();
		//auto dur = (end - start).count();

		/*if ((dur / 1000000) < 16)
			SDL_Delay(16 - (dur / 1000000));*/
	}

	SDL_DestroyTexture(tex);
	SDL_DestroyRenderer(rend);
	SDL_DestroyWindow(window);

	return 0;
}
<|endoftext|>