<commit_before>
#include "ksp_plugin/planetarium.hpp"

#include <vector>

#include "geometry/point.hpp"
#include "quantities/elementary_functions.hpp"

namespace principia {
namespace ksp_plugin {
namespace internal_planetarium {

using geometry::Position;
using geometry::RP2Line;
using geometry::Velocity;
using quantities::Pow;
using quantities::Sin;
using quantities::Sqrt;
using quantities::Tan;
using quantities::Time;

Planetarium::Parameters::Parameters(double const sphere_radius_multiplier,
                                    Angle const& angular_resolution,
                                    Angle const& field_of_view)
    : sphere_radius_multiplier_(sphere_radius_multiplier),
      sin²_angular_resolution_(Pow<2>(Sin(angular_resolution))),
      tan_angular_resolution_(Tan(angular_resolution)),
      tan_field_of_view_(Tan(field_of_view)) {}

Planetarium::Planetarium(
    Parameters const& parameters,
    Perspective<Navigation, Camera, Length, OrthogonalMap> const& perspective,
    not_null<Ephemeris<Barycentric> const*> const ephemeris,
    not_null<NavigationFrame const*> const plotting_frame)
    : parameters_(parameters),
      perspective_(perspective),
      ephemeris_(ephemeris),
      plotting_frame_(plotting_frame) {}

RP2Lines<Length, Camera> Planetarium::PlotMethod0(
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end,
    Instant const& now) const {
  auto const plottable_spheres = ComputePlottableSpheres(now);
  auto const plottable_segments = ComputePlottableSegments(plottable_spheres,
                                                           begin, end);

  auto const field_of_view_radius² =
      perspective_.focal() * perspective_.focal() *
      parameters_.tan_field_of_view_ * parameters_.tan_field_of_view_;
  std::experimental::optional<Position<Navigation>> previous_position;
  RP2Lines<Length, Camera> rp2_lines;
  for (auto const& plottable_segment : plottable_segments) {
    // Apply the projection to the current plottable segment.
    auto const rp2_first = perspective_(plottable_segment.first);
    auto const rp2_second = perspective_(plottable_segment.second);

    // If the segment is entirely outside the field of view, ignore it.
    Length const x1 = rp2_first.x();
    Length const y1 = rp2_first.y();
    Length const x2 = rp2_second.x();
    Length const y2 = rp2_second.y();
    if (x1 * x1 + y1 * y1 > field_of_view_radius² &&
        x2 * x2 + y2 * y2 > field_of_view_radius²) {
      continue;
    }

    // Create a new ℝP² line when two visible_segments are not consecutive.  Don't
    // compare ℝP² points for equality, that's expensive.
    bool const are_consecutive =
        previous_position == plottable_segment.first;
    previous_position = plottable_segment.second;

    if (are_consecutive) {
      rp2_lines.back().push_back(rp2_second);
    } else {
      RP2Line<Length, Camera> const rp2_line = {rp2_first, rp2_second};
      rp2_lines.push_back(rp2_line);
    }
  }
  return rp2_lines;
}

RP2Lines<Length, Camera> Planetarium::PlotMethod1(
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end,
    Instant const& now) const {
  Length const focal_plane_tolerance =
      perspective_.focal() * parameters_.tan_angular_resolution_;
  auto const focal_plane_tolerance² =
      focal_plane_tolerance * focal_plane_tolerance;

  auto const rp2_lines = PlotMethod0(begin, end, now);

  int skipped = 0;
  int total = 0;
  RP2Lines<Length, Camera> new_rp2_lines;
  for (auto const& rp2_line : rp2_lines) {
    RP2Line<Length, Camera> new_rp2_line;
    std::experimental::optional<RP2Point<Length, Camera>> start_rp2_point;
    for (int i = 0; i < rp2_line.size(); ++i) {
      RP2Point<Length, Camera> const& rp2_point = rp2_line[i];
      if (i == 0) {
        new_rp2_line.push_back(rp2_point);
        start_rp2_point = rp2_point;
      } else if (Pow<2>(rp2_point.x() - start_rp2_point->x()) +
                 Pow<2>(rp2_point.y() - start_rp2_point->y()) >
                     focal_plane_tolerance²) {
        // TODO(phl): This creates a segment if the tolerance is exceeded.  It
        // should probably create a segment that stays just below the tolerance.
        new_rp2_line.push_back(rp2_point);
        start_rp2_point = rp2_point;
      } else if (i == rp2_line.size() - 1) {
        new_rp2_line.push_back(rp2_point);
      } else {
        ++skipped;
      }
      ++total;
    }
    new_rp2_lines.push_back(std::move(new_rp2_line));
  }
  LOG(INFO) << "PlotMethod1 skipped " << skipped << " points out of " << total
            << ".";
  return new_rp2_lines;
}

RP2Lines<Length, Camera> Planetarium::PlotMethod2(
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end,
    Instant const& now) const {
  RP2Lines<Length, Camera> lines;
  if (begin == end) {
    return lines;
  }
  auto last = end;
  --last;
  if (begin == last) {
    return lines;
  }

  auto const plottable_spheres = ComputePlottableSpheres(now);

  auto const final_time = last.time();
  auto const& trajectory = *begin.trajectory();

  double const tan²_angular_resolution =
      Pow<2>(parameters_.tan_angular_resolution_);

  auto previous_time = begin.time();
  auto previous_degrees_of_freedom = begin.degrees_of_freedom();

  Position<Navigation> previous_position;
  Velocity<Navigation> previous_velocity;
  Time Δt;

  previous_position =
      plotting_frame_->ToThisFrameAtTime(previous_time)
          .rigid_transformation()(previous_degrees_of_freedom.position());
  previous_velocity =
      plotting_frame_->ToThisFrameAtTime(previous_time)
          .orthogonal_map()(previous_degrees_of_freedom.velocity());

  Δt = final_time - previous_time;

  Instant t;
  double estimated_tan²_error;
  Position<Navigation> position;

  std::experimental::optional<Position<Navigation>> last_endpoint;

  int steps_accepted = 0;
  int steps_attempted = 0;

  goto estimate_tan²_error;

  while (previous_time < final_time) {
    do {
      Δt *= 0.9 * Sqrt(Sqrt(tan²_angular_resolution / estimated_tan²_error));
    estimate_tan²_error:
      t = previous_time + Δt;
      if (t > final_time) {
        t = final_time;
        Δt = t - previous_time;
      }
      Position<Navigation> const estimated_position =
          previous_position + previous_velocity * Δt;

      position = plotting_frame_->ToThisFrameAtTime(t).rigid_transformation()(
                     trajectory.EvaluatePosition(t));

      estimated_tan²_error =
          perspective_.Tan²AngularDistance(estimated_position,
                                           position);
      ++steps_attempted;
    } while (estimated_tan²_error > tan²_angular_resolution);
    ++steps_accepted;

    // TODO(egg): also limit to field of view.
    auto const segment_behind_focal_plane =
        perspective_.SegmentBehindFocalPlane(
            Segment<Displacement<Navigation>>(previous_position, position));

    previous_position = position;
    previous_velocity = plotting_frame_->ToThisFrameAtTime(t).orthogonal_map()(
                            trajectory.EvaluateVelocity(t));
    previous_time = t;

    if (!segment_behind_focal_plane) {
      continue;
    }

    auto const visible_segments = perspective_.VisibleSegments(
                                      *segment_behind_focal_plane,
                                      plottable_spheres);
    for (auto const& segment : visible_segments) {
      if (last_endpoint != segment.first) {
        lines.emplace_back();
        lines.back().push_back(perspective_(segment.first));
      }
      lines.back().push_back(perspective_(segment.second));
      last_endpoint = segment.second;
    }
  }
  LOG(INFO) << "PlotMethod2 took " << steps_accepted << " steps, attempted "
            << steps_attempted << " steps.";
  return lines;
}

std::vector<Sphere<Length, Navigation>> Planetarium::ComputePlottableSpheres(
    Instant const& now) const {
  RigidMotion<Barycentric, Navigation> const rigid_motion_at_now =
      plotting_frame_->ToThisFrameAtTime(now);
  std::vector<Sphere<Length, Navigation>> plottable_spheres;

  auto const& bodies = ephemeris_->bodies();
  for (auto const body : bodies) {
    auto const trajectory = ephemeris_->trajectory(body);
    Length const mean_radius = body->mean_radius();
    Position<Barycentric> const centre_in_barycentric =
        trajectory->EvaluatePosition(now);
    Sphere<Length, Navigation> plottable_sphere(
        rigid_motion_at_now.rigid_transformation()(centre_in_barycentric),
        parameters_.sphere_radius_multiplier_ * mean_radius);
    // If the sphere is seen under an angle that is very small it doesn't
    // participate in hiding.
    if (perspective_.SphereSin²HalfAngle(plottable_sphere) >
        parameters_.sin²_angular_resolution_) {
      plottable_spheres.emplace_back(std::move(plottable_sphere));
    }
  }
  return plottable_spheres;
}

Segments<Displacement<Navigation>> Planetarium::ComputePlottableSegments(
    const std::vector<Sphere<Length, Navigation>>& plottable_spheres,
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end) const {
  std::vector<Segment<Displacement<Navigation>>> all_segments;
  if (begin == end) {
    return all_segments;
  }
  auto it1 = begin;
  Instant t1 = it1.time();
  RigidMotion<Barycentric, Navigation> rigid_motion_at_t1 =
      plotting_frame_->ToThisFrameAtTime(t1);
  Position<Navigation> p1 =
      rigid_motion_at_t1(it1.degrees_of_freedom()).position();

  auto it2 = it1;
  while (++it2 != end) {
    // Processing one segment of the trajectory.
    Instant const t2 = it2.time();

    // Transform the degrees of freedom to the plotting frame.
    RigidMotion<Barycentric, Navigation> const rigid_motion_at_t2 =
        plotting_frame_->ToThisFrameAtTime(t2);
    Position<Navigation> const p2 =
        rigid_motion_at_t2(it2.degrees_of_freedom()).position();

    // Find the part of the segment that is behind the focal plane.  We don't
    // care about things that are in front of the focal plane.
    const Segment<Displacement<Navigation>> segment = {p1, p2};
    auto const segment_behind_focal_plane =
        perspective_.SegmentBehindFocalPlane(segment);
    if (segment_behind_focal_plane) {
      // Find the part(s) of the segment that are not hidden by spheres.  These
      // are the ones we want to plot.
      auto segments = perspective_.VisibleSegments(*segment_behind_focal_plane,
                                                   plottable_spheres);
      std::move(segments.begin(),
                segments.end(),
                std::back_inserter(all_segments));
    }

    it1 = it2;
    t1 = t2;
    rigid_motion_at_t1 = rigid_motion_at_t2;
    p1 = p2;
  }

  return all_segments;
}

}  // namespace internal_planetarium
}  // namespace ksp_plugin
}  // namespace principia
<commit_msg>I did it again<commit_after>
#include "ksp_plugin/planetarium.hpp"

#include <vector>

#include "geometry/point.hpp"
#include "quantities/elementary_functions.hpp"

namespace principia {
namespace ksp_plugin {
namespace internal_planetarium {

using geometry::Position;
using geometry::RP2Line;
using geometry::Velocity;
using quantities::Pow;
using quantities::Sin;
using quantities::Sqrt;
using quantities::Tan;
using quantities::Time;

Planetarium::Parameters::Parameters(double const sphere_radius_multiplier,
                                    Angle const& angular_resolution,
                                    Angle const& field_of_view)
    : sphere_radius_multiplier_(sphere_radius_multiplier),
      sin²_angular_resolution_(Pow<2>(Sin(angular_resolution))),
      tan_angular_resolution_(Tan(angular_resolution)),
      tan_field_of_view_(Tan(field_of_view)) {}

Planetarium::Planetarium(
    Parameters const& parameters,
    Perspective<Navigation, Camera, Length, OrthogonalMap> const& perspective,
    not_null<Ephemeris<Barycentric> const*> const ephemeris,
    not_null<NavigationFrame const*> const plotting_frame)
    : parameters_(parameters),
      perspective_(perspective),
      ephemeris_(ephemeris),
      plotting_frame_(plotting_frame) {}

RP2Lines<Length, Camera> Planetarium::PlotMethod0(
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end,
    Instant const& now) const {
  auto const plottable_spheres = ComputePlottableSpheres(now);
  auto const plottable_segments = ComputePlottableSegments(plottable_spheres,
                                                           begin, end);

  auto const field_of_view_radius² =
      perspective_.focal() * perspective_.focal() *
      parameters_.tan_field_of_view_ * parameters_.tan_field_of_view_;
  std::experimental::optional<Position<Navigation>> previous_position;
  RP2Lines<Length, Camera> rp2_lines;
  for (auto const& plottable_segment : plottable_segments) {
    // Apply the projection to the current plottable segment.
    auto const rp2_first = perspective_(plottable_segment.first);
    auto const rp2_second = perspective_(plottable_segment.second);

    // If the segment is entirely outside the field of view, ignore it.
    Length const x1 = rp2_first.x();
    Length const y1 = rp2_first.y();
    Length const x2 = rp2_second.x();
    Length const y2 = rp2_second.y();
    if (x1 * x1 + y1 * y1 > field_of_view_radius² &&
        x2 * x2 + y2 * y2 > field_of_view_radius²) {
      continue;
    }

    // Create a new ℝP² line when two visible_segments are not consecutive.  Don't
    // compare ℝP² points for equality, that's expensive.
    bool const are_consecutive =
        previous_position == plottable_segment.first;
    previous_position = plottable_segment.second;

    if (are_consecutive) {
      rp2_lines.back().push_back(rp2_second);
    } else {
      RP2Line<Length, Camera> const rp2_line = {rp2_first, rp2_second};
      rp2_lines.push_back(rp2_line);
    }
  }
  return rp2_lines;
}

RP2Lines<Length, Camera> Planetarium::PlotMethod1(
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end,
    Instant const& now) const {
  Length const focal_plane_tolerance =
      perspective_.focal() * parameters_.tan_angular_resolution_;
  auto const focal_plane_tolerance² =
      focal_plane_tolerance * focal_plane_tolerance;

  auto const rp2_lines = PlotMethod0(begin, end, now);

  int skipped = 0;
  int total = 0;
  RP2Lines<Length, Camera> new_rp2_lines;
  for (auto const& rp2_line : rp2_lines) {
    RP2Line<Length, Camera> new_rp2_line;
    std::experimental::optional<RP2Point<Length, Camera>> start_rp2_point;
    for (int i = 0; i < rp2_line.size(); ++i) {
      RP2Point<Length, Camera> const& rp2_point = rp2_line[i];
      if (i == 0) {
        new_rp2_line.push_back(rp2_point);
        start_rp2_point = rp2_point;
      } else if (Pow<2>(rp2_point.x() - start_rp2_point->x()) +
                 Pow<2>(rp2_point.y() - start_rp2_point->y()) >
                     focal_plane_tolerance²) {
        // TODO(phl): This creates a segment if the tolerance is exceeded.  It
        // should probably create a segment that stays just below the tolerance.
        new_rp2_line.push_back(rp2_point);
        start_rp2_point = rp2_point;
      } else if (i == rp2_line.size() - 1) {
        new_rp2_line.push_back(rp2_point);
      } else {
        ++skipped;
      }
      ++total;
    }
    new_rp2_lines.push_back(std::move(new_rp2_line));
  }
  LOG(INFO) << "PlotMethod1 skipped " << skipped << " points out of " << total
            << ".";
  return new_rp2_lines;
}

RP2Lines<Length, Camera> Planetarium::PlotMethod2(
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end,
    Instant const& now) const {
  RP2Lines<Length, Camera> lines;
  if (begin == end) {
    return lines;
  }
  auto last = end;
  --last;
  if (begin == last) {
    return lines;
  }

  auto const plottable_spheres = ComputePlottableSpheres(now);

  auto const final_time = last.time();
  auto const& trajectory = *begin.trajectory();

  double const tan²_angular_resolution =
      Pow<2>(parameters_.tan_angular_resolution_);

  auto previous_time = begin.time();
  auto previous_degrees_of_freedom = begin.degrees_of_freedom();

  Position<Navigation> previous_position;
  Velocity<Navigation> previous_velocity;
  Time Δt;

  previous_position =
      plotting_frame_->ToThisFrameAtTime(previous_time)
          .rigid_transformation()(previous_degrees_of_freedom.position());
  previous_velocity =
      plotting_frame_->ToThisFrameAtTime(previous_time)
          .orthogonal_map()(previous_degrees_of_freedom.velocity());

  Δt = final_time - previous_time;

  Instant t;
  double estimated_tan²_error;
  Position<Navigation> position;

  std::experimental::optional<Position<Navigation>> last_endpoint;

  int steps_accepted = 0;
  int steps_attempted = 0;

  goto estimate_tan²_error;

  while (previous_time < final_time) {
    do {
      Δt *= 0.9 * Sqrt(Sqrt(tan²_angular_resolution / estimated_tan²_error));
    estimate_tan²_error:
      t = previous_time + Δt;
      if (t > final_time) {
        t = final_time;
        Δt = t - previous_time;
      }
      Position<Navigation> const estimated_position =
          previous_position + previous_velocity * Δt;

      position = plotting_frame_->ToThisFrameAtTime(t).rigid_transformation()(
                     trajectory.EvaluatePosition(t));

      estimated_tan²_error =
          perspective_.Tan²AngularDistance(estimated_position, position) / 16;
      ++steps_attempted;
    } while (estimated_tan²_error > tan²_angular_resolution);
    ++steps_accepted;

    // TODO(egg): also limit to field of view.
    auto const segment_behind_focal_plane =
        perspective_.SegmentBehindFocalPlane(
            Segment<Displacement<Navigation>>(previous_position, position));

    previous_position = position;
    previous_velocity = plotting_frame_->ToThisFrameAtTime(t).orthogonal_map()(
                            trajectory.EvaluateVelocity(t));
    previous_time = t;

    if (!segment_behind_focal_plane) {
      continue;
    }

    auto const visible_segments = perspective_.VisibleSegments(
                                      *segment_behind_focal_plane,
                                      plottable_spheres);
    for (auto const& segment : visible_segments) {
      if (last_endpoint != segment.first) {
        lines.emplace_back();
        lines.back().push_back(perspective_(segment.first));
      }
      lines.back().push_back(perspective_(segment.second));
      last_endpoint = segment.second;
    }
  }
  LOG(INFO) << "PlotMethod2 took " << steps_accepted << " steps, attempted "
            << steps_attempted << " steps.";
  return lines;
}

std::vector<Sphere<Length, Navigation>> Planetarium::ComputePlottableSpheres(
    Instant const& now) const {
  RigidMotion<Barycentric, Navigation> const rigid_motion_at_now =
      plotting_frame_->ToThisFrameAtTime(now);
  std::vector<Sphere<Length, Navigation>> plottable_spheres;

  auto const& bodies = ephemeris_->bodies();
  for (auto const body : bodies) {
    auto const trajectory = ephemeris_->trajectory(body);
    Length const mean_radius = body->mean_radius();
    Position<Barycentric> const centre_in_barycentric =
        trajectory->EvaluatePosition(now);
    Sphere<Length, Navigation> plottable_sphere(
        rigid_motion_at_now.rigid_transformation()(centre_in_barycentric),
        parameters_.sphere_radius_multiplier_ * mean_radius);
    // If the sphere is seen under an angle that is very small it doesn't
    // participate in hiding.
    if (perspective_.SphereSin²HalfAngle(plottable_sphere) >
        parameters_.sin²_angular_resolution_) {
      plottable_spheres.emplace_back(std::move(plottable_sphere));
    }
  }
  return plottable_spheres;
}

Segments<Displacement<Navigation>> Planetarium::ComputePlottableSegments(
    const std::vector<Sphere<Length, Navigation>>& plottable_spheres,
    DiscreteTrajectory<Barycentric>::Iterator const& begin,
    DiscreteTrajectory<Barycentric>::Iterator const& end) const {
  std::vector<Segment<Displacement<Navigation>>> all_segments;
  if (begin == end) {
    return all_segments;
  }
  auto it1 = begin;
  Instant t1 = it1.time();
  RigidMotion<Barycentric, Navigation> rigid_motion_at_t1 =
      plotting_frame_->ToThisFrameAtTime(t1);
  Position<Navigation> p1 =
      rigid_motion_at_t1(it1.degrees_of_freedom()).position();

  auto it2 = it1;
  while (++it2 != end) {
    // Processing one segment of the trajectory.
    Instant const t2 = it2.time();

    // Transform the degrees of freedom to the plotting frame.
    RigidMotion<Barycentric, Navigation> const rigid_motion_at_t2 =
        plotting_frame_->ToThisFrameAtTime(t2);
    Position<Navigation> const p2 =
        rigid_motion_at_t2(it2.degrees_of_freedom()).position();

    // Find the part of the segment that is behind the focal plane.  We don't
    // care about things that are in front of the focal plane.
    const Segment<Displacement<Navigation>> segment = {p1, p2};
    auto const segment_behind_focal_plane =
        perspective_.SegmentBehindFocalPlane(segment);
    if (segment_behind_focal_plane) {
      // Find the part(s) of the segment that are not hidden by spheres.  These
      // are the ones we want to plot.
      auto segments = perspective_.VisibleSegments(*segment_behind_focal_plane,
                                                   plottable_spheres);
      std::move(segments.begin(),
                segments.end(),
                std::back_inserter(all_segments));
    }

    it1 = it2;
    t1 = t2;
    rigid_motion_at_t1 = rigid_motion_at_t2;
    p1 = p2;
  }

  return all_segments;
}

}  // namespace internal_planetarium
}  // namespace ksp_plugin
}  // namespace principia
<|endoftext|>


<commit_before>#include "Method.hpp"

namespace autobind {

//
// void Method::codegenCall(std::ostream &out) const
// {
// 	out << "self->object." << name();
// 	codegenCallArgs(out);
// 	out << ";\n";
// }
//
//
// void Method::codegenMethodTable(std::ostream &out) const
// {
// 	auto docstring = processDocString(this->docstring());
//
// 	out << boost::format("{\"%1%\", (PyCFunction) %2%, METH_VARARGS, %3%},\n")
// 		% unqualifiedName()
// 		% implName()
// 		% ("\"" + docstring + "\"");
// }
//
} // autobind

<commit_msg>Remove the old Method.cpp file, which was empty.<commit_after><|endoftext|>


<commit_before>#include "common/params.h"

#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif  // _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <dirent.h>
#include <sys/file.h>
#include <sys/stat.h>

#include <map>
#include <string>
#include <iostream>
#include <csignal>
#include <string.h>

#include "common/util.h"
#include "common/utilpp.h"

std::string getenv_default(const char* env_var, const char * suffix, const char* default_val) {
  const char* env_val = getenv(env_var);
  if (env_val != NULL){
    return std::string(env_val) + std::string(suffix);
  } else{
    return std::string(default_val);
  }
}

#if defined(QCOM) || defined(QCOM2)
const std::string default_params_path = "/data/params";
#else
const std::string default_params_path = getenv_default("BASEDIR", "persists/params", "/data/params");
#endif

#if defined(QCOM) || defined(QCOM2)
const std::string persistent_params_path = "/persist/comma/params";
#else
const std::string persistent_params_path = default_params_path;
#endif

volatile sig_atomic_t params_do_exit = 0;
void params_sig_handler(int signal) {
  std::cout << "got signal" << std::endl;
  params_do_exit = 1;
}

static int fsync_dir(const char* path){
  int result = 0;
  int fd = open(path, O_RDONLY, 0755);

  if (fd < 0){
    result = -1;
    goto cleanup;
  }

  result = fsync(fd);
  if (result < 0) {
    goto cleanup;
  }

 cleanup:
  int result_close = 0;
  if (fd >= 0){
    result_close = close(fd);
  }

  if (result_close < 0) {
    return result_close;
  } else {
    return result;
  }
}

static int mkdir_p(std::string path) {
  char * _path = (char *)path.c_str();

  mode_t prev_mask = umask(0);
  for (char *p = _path + 1; *p; p++) {
    if (*p == '/') {
      *p = '\0'; // Temporarily truncate
      if (mkdir(_path, 0777) != 0) {
        if (errno != EEXIST) return -1;
      }
      *p = '/';
    }
  }
  if (mkdir(_path, 0777) != 0) {
    if (errno != EEXIST) return -1;
  }
  chmod(_path, 0777);
  umask(prev_mask);
  return 0;
}

static int ensure_dir_exists(std::string path) {
  // TODO: replace by std::filesystem::create_directories
  return mkdir_p(path.c_str());
}

Params::Params(bool persistent_param){
  params_path = persistent_param ? persistent_params_path : default_params_path;
}

Params::Params(std::string path) {
  params_path = path;
}

int Params::write_db_value(std::string key, std::string dat){
  return write_db_value(key.c_str(), dat.c_str(), dat.length());
}

int Params::write_db_value(const char* key, const char* value, size_t value_size) {
  // Information about safely and atomically writing a file: https://lwn.net/Articles/457667/
  // 1) Create temp file
  // 2) Write data to temp file
  // 3) fsync() the temp file
  // 4) rename the temp file to the real name
  // 5) fsync() the containing directory

  int lock_fd = -1;
  int tmp_fd = -1;
  int result;
  std::string path;
  std::string tmp_path;
  ssize_t bytes_written;

  // Make sure params path exists
  result = ensure_dir_exists(params_path);
  if (result < 0) {
    goto cleanup;
  }

  // See if the symlink exists, otherwise create it
  path = params_path + "/d";
  struct stat st;
  if (stat(path.c_str(), &st) == -1) {
    // Create temp folder
    path = params_path + "/.tmp_XXXXXX";

    char *t = mkdtemp((char*)path.c_str());
    if (t == NULL){
      goto cleanup;
    }
    std::string tmp_dir(t);

    // Set permissions
    result = chmod(tmp_dir.c_str(), 0777);
    if (result < 0) {
      goto cleanup;
    }

    // Symlink it to temp link
    tmp_path = tmp_dir + ".link";
    result = symlink(tmp_dir.c_str(), tmp_path.c_str());
    if (result < 0) {
      goto cleanup;
    }

    // Move symlink to <params>/d
    path = params_path + "/d";
    result = rename(tmp_path.c_str(), path.c_str());
    if (result < 0) {
      goto cleanup;
    }
  } else {
    // Ensure permissions are correct in case we didn't create the symlink
    result = chmod(path.c_str(), 0777);
    if (result < 0) {
      goto cleanup;
    }
  }

  // Write value to temp.
  tmp_path = params_path + "/.tmp_value_XXXXXX";
  tmp_fd = mkstemp((char*)tmp_path.c_str());
  bytes_written = write(tmp_fd, value, value_size);
  if (bytes_written < 0 || (size_t)bytes_written != value_size) {
    result = -20;
    goto cleanup;
  }

  // Build lock path
  path = params_path + "/.lock";
  lock_fd = open(path.c_str(), O_CREAT, 0775);

  // Build key path
  path = params_path + "/d/" + std::string(key);

  // Take lock.
  result = flock(lock_fd, LOCK_EX);
  if (result < 0) {
    goto cleanup;
  }

  // change permissions to 0666 for apks
  result = fchmod(tmp_fd, 0666);
  if (result < 0) {
    goto cleanup;
  }

  // fsync to force persist the changes.
  result = fsync(tmp_fd);
  if (result < 0) {
    goto cleanup;
  }

  // Move temp into place.
  result = rename(tmp_path.c_str(), path.c_str());
  if (result < 0) {
    goto cleanup;
  }

  // fsync parent directory
  path = params_path + "/d";
  result = fsync_dir(path.c_str());
  if (result < 0) {
    goto cleanup;
  }

cleanup:
  // Release lock.
  if (lock_fd >= 0) {
    close(lock_fd);
  }
  if (tmp_fd >= 0) {
    if (result < 0) {
      remove(tmp_path.c_str());
    }
    close(tmp_fd);
  }
  return result;
}

int Params::delete_db_value(std::string key) {
  return delete_db_value(key.c_str());
}

int Params::delete_db_value(const char* key) {
  int lock_fd = -1;
  int result;
  std::string path;

  // Build lock path, and open lockfile
  path = params_path + "/.lock";
  lock_fd = open(path.c_str(), O_CREAT, 0775);

  // Take lock.
  result = flock(lock_fd, LOCK_EX);
  if (result < 0) {
    goto cleanup;
  }

  // Delete value.
  path = params_path + "/d/" + std::string(key);
  result = remove(path.c_str());
  if (result != 0) {
    result = ERR_NO_VALUE;
    goto cleanup;
  }

  // fsync parent directory
  path = params_path + "/d";
  result = fsync_dir(path.c_str());
  if (result < 0) {
    goto cleanup;
  }

cleanup:
  // Release lock.
  if (lock_fd >= 0) {
    close(lock_fd);
  }
  return result;
}

std::string Params::get(std::string key, bool block){
  char* value;
  size_t size;
  int r;

  if (block){
    r = read_db_value_blocking((const char*)key.c_str(), &value, &size);
  } else {
    r = read_db_value((const char*)key.c_str(), &value, &size);
  }

  if (r == 0){
    std::string s(value, size);
    free(value);
    return s;
  } else {
    return "";
  }
}

int Params::read_db_value(const char* key, char** value, size_t* value_sz) {
  std::string path = params_path + "/d/" + std::string(key);
  *value = static_cast<char*>(read_file(path.c_str(), value_sz));
  if (*value == NULL) {
    return -22;
  }
  return 0;
}

int Params::read_db_value_blocking(const char* key, char** value, size_t* value_sz) {
  params_do_exit = 0;
  void (*prev_handler_sigint)(int) = std::signal(SIGINT, params_sig_handler);
  void (*prev_handler_sigterm)(int) = std::signal(SIGTERM, params_sig_handler);

  while (!params_do_exit) {
    const int result = read_db_value(key, value, value_sz);
    if (result == 0) {
      break;
    } else {
      usleep(100000); // 0.1 s
    }
  }

  std::signal(SIGINT, prev_handler_sigint);
  std::signal(SIGTERM, prev_handler_sigterm);
  return params_do_exit; // Return 0 if we had no interrupt
}

int Params::read_db_all(std::map<std::string, std::string> *params) {
  int err = 0;

  std::string lock_path = params_path + "/.lock";

  int lock_fd = open(lock_path.c_str(), 0);
  if (lock_fd < 0) return -1;

  err = flock(lock_fd, LOCK_SH);
  if (err < 0) {
    close(lock_fd);
    return err;
  }

  std::string key_path = params_path + "/d";
  DIR *d = opendir(key_path.c_str());
  if (!d) {
    close(lock_fd);
    return -1;
  }

  struct dirent *de = NULL;
  while ((de = readdir(d))) {
    if (!isalnum(de->d_name[0])) continue;
    std::string key = std::string(de->d_name);
    std::string value = util::read_file(key_path + "/" + key);

    (*params)[key] = value;
  }

  closedir(d);

  close(lock_fd);
  return 0;
}

std::vector<char> Params::read_db_bytes(const char* param_name) {
  std::vector<char> bytes;
  char* value;
  size_t sz;
  int result = read_db_value(param_name, &value, &sz);
  if (result == 0) {
    bytes.assign(value, value+sz);
    free(value);
  }
  return bytes;
}

bool Params::read_db_bool(const char* param_name) {
  std::vector<char> bytes = read_db_bytes(param_name);
  return bytes.size() > 0 and bytes[0] == '1';
}
<commit_msg>fix typo in params path on PC<commit_after>#include "common/params.h"

#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif  // _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <dirent.h>
#include <sys/file.h>
#include <sys/stat.h>

#include <map>
#include <string>
#include <iostream>
#include <csignal>
#include <string.h>

#include "common/util.h"
#include "common/utilpp.h"

std::string getenv_default(const char* env_var, const char * suffix, const char* default_val) {
  const char* env_val = getenv(env_var);
  if (env_val != NULL){
    return std::string(env_val) + std::string(suffix);
  } else{
    return std::string(default_val);
  }
}

#if defined(QCOM) || defined(QCOM2)
const std::string default_params_path = "/data/params";
#else
const std::string default_params_path = getenv_default("BASEDIR", "/persist/params", "/data/params");
#endif

#if defined(QCOM) || defined(QCOM2)
const std::string persistent_params_path = "/persist/comma/params";
#else
const std::string persistent_params_path = default_params_path;
#endif

volatile sig_atomic_t params_do_exit = 0;
void params_sig_handler(int signal) {
  params_do_exit = 1;
}

static int fsync_dir(const char* path){
  int result = 0;
  int fd = open(path, O_RDONLY, 0755);

  if (fd < 0){
    result = -1;
    goto cleanup;
  }

  result = fsync(fd);
  if (result < 0) {
    goto cleanup;
  }

 cleanup:
  int result_close = 0;
  if (fd >= 0){
    result_close = close(fd);
  }

  if (result_close < 0) {
    return result_close;
  } else {
    return result;
  }
}

static int mkdir_p(std::string path) {
  char * _path = (char *)path.c_str();

  mode_t prev_mask = umask(0);
  for (char *p = _path + 1; *p; p++) {
    if (*p == '/') {
      *p = '\0'; // Temporarily truncate
      if (mkdir(_path, 0777) != 0) {
        if (errno != EEXIST) return -1;
      }
      *p = '/';
    }
  }
  if (mkdir(_path, 0777) != 0) {
    if (errno != EEXIST) return -1;
  }
  chmod(_path, 0777);
  umask(prev_mask);
  return 0;
}

static int ensure_dir_exists(std::string path) {
  // TODO: replace by std::filesystem::create_directories
  return mkdir_p(path.c_str());
}

Params::Params(bool persistent_param){
  params_path = persistent_param ? persistent_params_path : default_params_path;
}

Params::Params(std::string path) {
  params_path = path;
}

int Params::write_db_value(std::string key, std::string dat){
  return write_db_value(key.c_str(), dat.c_str(), dat.length());
}

int Params::write_db_value(const char* key, const char* value, size_t value_size) {
  // Information about safely and atomically writing a file: https://lwn.net/Articles/457667/
  // 1) Create temp file
  // 2) Write data to temp file
  // 3) fsync() the temp file
  // 4) rename the temp file to the real name
  // 5) fsync() the containing directory

  int lock_fd = -1;
  int tmp_fd = -1;
  int result;
  std::string path;
  std::string tmp_path;
  ssize_t bytes_written;

  // Make sure params path exists
  result = ensure_dir_exists(params_path);
  if (result < 0) {
    goto cleanup;
  }

  // See if the symlink exists, otherwise create it
  path = params_path + "/d";
  struct stat st;
  if (stat(path.c_str(), &st) == -1) {
    // Create temp folder
    path = params_path + "/.tmp_XXXXXX";

    char *t = mkdtemp((char*)path.c_str());
    if (t == NULL){
      goto cleanup;
    }
    std::string tmp_dir(t);

    // Set permissions
    result = chmod(tmp_dir.c_str(), 0777);
    if (result < 0) {
      goto cleanup;
    }

    // Symlink it to temp link
    tmp_path = tmp_dir + ".link";
    result = symlink(tmp_dir.c_str(), tmp_path.c_str());
    if (result < 0) {
      goto cleanup;
    }

    // Move symlink to <params>/d
    path = params_path + "/d";
    result = rename(tmp_path.c_str(), path.c_str());
    if (result < 0) {
      goto cleanup;
    }
  } else {
    // Ensure permissions are correct in case we didn't create the symlink
    result = chmod(path.c_str(), 0777);
    if (result < 0) {
      goto cleanup;
    }
  }

  // Write value to temp.
  tmp_path = params_path + "/.tmp_value_XXXXXX";
  tmp_fd = mkstemp((char*)tmp_path.c_str());
  bytes_written = write(tmp_fd, value, value_size);
  if (bytes_written < 0 || (size_t)bytes_written != value_size) {
    result = -20;
    goto cleanup;
  }

  // Build lock path
  path = params_path + "/.lock";
  lock_fd = open(path.c_str(), O_CREAT, 0775);

  // Build key path
  path = params_path + "/d/" + std::string(key);

  // Take lock.
  result = flock(lock_fd, LOCK_EX);
  if (result < 0) {
    goto cleanup;
  }

  // change permissions to 0666 for apks
  result = fchmod(tmp_fd, 0666);
  if (result < 0) {
    goto cleanup;
  }

  // fsync to force persist the changes.
  result = fsync(tmp_fd);
  if (result < 0) {
    goto cleanup;
  }

  // Move temp into place.
  result = rename(tmp_path.c_str(), path.c_str());
  if (result < 0) {
    goto cleanup;
  }

  // fsync parent directory
  path = params_path + "/d";
  result = fsync_dir(path.c_str());
  if (result < 0) {
    goto cleanup;
  }

cleanup:
  // Release lock.
  if (lock_fd >= 0) {
    close(lock_fd);
  }
  if (tmp_fd >= 0) {
    if (result < 0) {
      remove(tmp_path.c_str());
    }
    close(tmp_fd);
  }
  return result;
}

int Params::delete_db_value(std::string key) {
  return delete_db_value(key.c_str());
}

int Params::delete_db_value(const char* key) {
  int lock_fd = -1;
  int result;
  std::string path;

  // Build lock path, and open lockfile
  path = params_path + "/.lock";
  lock_fd = open(path.c_str(), O_CREAT, 0775);

  // Take lock.
  result = flock(lock_fd, LOCK_EX);
  if (result < 0) {
    goto cleanup;
  }

  // Delete value.
  path = params_path + "/d/" + std::string(key);
  result = remove(path.c_str());
  if (result != 0) {
    result = ERR_NO_VALUE;
    goto cleanup;
  }

  // fsync parent directory
  path = params_path + "/d";
  result = fsync_dir(path.c_str());
  if (result < 0) {
    goto cleanup;
  }

cleanup:
  // Release lock.
  if (lock_fd >= 0) {
    close(lock_fd);
  }
  return result;
}

std::string Params::get(std::string key, bool block){
  char* value;
  size_t size;
  int r;

  if (block){
    r = read_db_value_blocking((const char*)key.c_str(), &value, &size);
  } else {
    r = read_db_value((const char*)key.c_str(), &value, &size);
  }

  if (r == 0){
    std::string s(value, size);
    free(value);
    return s;
  } else {
    return "";
  }
}

int Params::read_db_value(const char* key, char** value, size_t* value_sz) {
  std::string path = params_path + "/d/" + std::string(key);
  *value = static_cast<char*>(read_file(path.c_str(), value_sz));
  if (*value == NULL) {
    return -22;
  }
  return 0;
}

int Params::read_db_value_blocking(const char* key, char** value, size_t* value_sz) {
  params_do_exit = 0;
  void (*prev_handler_sigint)(int) = std::signal(SIGINT, params_sig_handler);
  void (*prev_handler_sigterm)(int) = std::signal(SIGTERM, params_sig_handler);

  while (!params_do_exit) {
    const int result = read_db_value(key, value, value_sz);
    if (result == 0) {
      break;
    } else {
      usleep(100000); // 0.1 s
    }
  }

  std::signal(SIGINT, prev_handler_sigint);
  std::signal(SIGTERM, prev_handler_sigterm);
  return params_do_exit; // Return 0 if we had no interrupt
}

int Params::read_db_all(std::map<std::string, std::string> *params) {
  int err = 0;

  std::string lock_path = params_path + "/.lock";

  int lock_fd = open(lock_path.c_str(), 0);
  if (lock_fd < 0) return -1;

  err = flock(lock_fd, LOCK_SH);
  if (err < 0) {
    close(lock_fd);
    return err;
  }

  std::string key_path = params_path + "/d";
  DIR *d = opendir(key_path.c_str());
  if (!d) {
    close(lock_fd);
    return -1;
  }

  struct dirent *de = NULL;
  while ((de = readdir(d))) {
    if (!isalnum(de->d_name[0])) continue;
    std::string key = std::string(de->d_name);
    std::string value = util::read_file(key_path + "/" + key);

    (*params)[key] = value;
  }

  closedir(d);

  close(lock_fd);
  return 0;
}

std::vector<char> Params::read_db_bytes(const char* param_name) {
  std::vector<char> bytes;
  char* value;
  size_t sz;
  int result = read_db_value(param_name, &value, &sz);
  if (result == 0) {
    bytes.assign(value, value+sz);
    free(value);
  }
  return bytes;
}

bool Params::read_db_bool(const char* param_name) {
  std::vector<char> bytes = read_db_bytes(param_name);
  return bytes.size() > 0 and bytes[0] == '1';
}
<|endoftext|>


<commit_before>#include <stdio.h>
#include <stdlib.h>
#include <eigen3/Eigen/Dense>

#include "common/visionbuf.h"
#include "common/visionipc.h"
#include "common/swaglog.h"

#include "models/driving.h"
#include "messaging.hpp"
volatile sig_atomic_t do_exit = 0;

static void set_do_exit(int sig) {
  do_exit = 1;
}

// globals
bool run_model;
mat3 cur_transform;
pthread_mutex_t transform_lock;

void* live_thread(void *arg) {
  int err;
  set_thread_name("live");

  SubMaster sm({"liveCalibration"});
  /*
     import numpy as np
     from common.transformations.model import medmodel_frame_from_road_frame
     medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
     ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
  */
  Eigen::Matrix<float, 3, 3> ground_from_medmodel_frame;
  ground_from_medmodel_frame <<
    0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
    -1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
    -1.84808520e-20, 9.00738606e-04,-4.28751576e-02;

  Eigen::Matrix<float, 3, 3> eon_intrinsics;
  eon_intrinsics <<
    910.0, 0.0, 582.0,
    0.0, 910.0, 437.0,
    0.0,   0.0,   1.0;

  while (!do_exit) {
    if (sm.update(10) > 0){

      auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix();
      Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
      for (int i = 0; i < 4*3; i++){
        extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
      }

      auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen;
      Eigen::Matrix<float, 3, 3> camera_frame_from_ground;
      camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
      camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
      camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3);

      auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame;

      pthread_mutex_lock(&transform_lock);
      for (int i=0; i<3*3; i++) {
        cur_transform.v[i] = warp_matrix(i / 3, i % 3);
      }

      run_model = true;
      pthread_mutex_unlock(&transform_lock);
    }
  }
  return NULL;
}

int main(int argc, char **argv) {
  int err;
  set_realtime_priority(51);

  // start calibration thread
  pthread_t live_thread_handle;
  err = pthread_create(&live_thread_handle, NULL, live_thread, NULL);
  assert(err == 0);

  // messaging
  PubMaster pm({"model", "cameraOdometry"});
  SubMaster sm({"pathPlan"});

#ifdef QCOM
  cl_device_type device_type = CL_DEVICE_TYPE_DEFAULT;
#else
  cl_device_type device_type = CL_DEVICE_TYPE_CPU;
#endif

  // cl init
  cl_device_id device_id;
  cl_context context;
  cl_command_queue q;
  {
    cl_uint num_platforms;
    err = clGetPlatformIDs(0, NULL, &num_platforms);
    assert(err == 0);

    cl_platform_id * platform_ids = new cl_platform_id[num_platforms];
    err = clGetPlatformIDs(num_platforms, platform_ids, NULL);
    assert(err == 0);

    LOGD("got %d opencl platform(s)", num_platforms);

    char cBuffer[1024];
    bool opencl_platform_found = false;

    for (size_t i = 0; i < num_platforms; i++){
      err = clGetPlatformInfo(platform_ids[i], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL);
      assert(err == 0);
      LOGD("platform[%zu] CL_PLATFORM_NAME: %s", i, cBuffer);

      cl_uint num_devices;
      err = clGetDeviceIDs(platform_ids[i], device_type, 0, NULL, &num_devices);
      if (err != 0|| !num_devices){
        continue;
      }

      // Get first device
      err = clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL);
      assert(err == 0);

      context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err);
      assert(err == 0);

      q = clCreateCommandQueue(context, device_id, 0, &err);
      assert(err == 0);

      opencl_platform_found = true;
      break;
    }

    delete[] platform_ids;

    if (!opencl_platform_found){
      LOGE("No valid openCL platform found");
      assert(opencl_platform_found);
    }

    LOGD("opencl init complete");
  }

  // init the models
  ModelState model;
  model_init(&model, device_id, context, true);
  LOGW("models loaded, modeld starting");

  // debayering does a 2x downscale
  mat3 yuv_transform = transform_scale_buffer((mat3){{
    1.0, 0.0, 0.0,
    0.0, 1.0, 0.0,
    0.0, 0.0, 1.0,
  }}, 0.5);

  // loop
  VisionStream stream;
  while (!do_exit) {
    VisionStreamBufs buf_info;
    err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info);
    if (err) {
      LOGW("visionstream connect failed");
      usleep(100000);
      continue;
    }
    LOGW("connected with buffer size: %d", buf_info.buf_len);

    // one frame in memory
    cl_mem yuv_cl;
    VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl);

    double last = 0;
    int desire = -1;
    while (!do_exit) {
      VIPCBuf *buf;
      VIPCBufExtra extra;
      buf = visionstream_get(&stream, &extra);
      if (buf == NULL) {
        LOGW("visionstream get failed");
        visionstream_destroy(&stream);
        break;
      }

      pthread_mutex_lock(&transform_lock);
      mat3 transform = cur_transform;
      const bool run_model_this_iter = run_model;
      pthread_mutex_unlock(&transform_lock);

      if (sm.update(0) > 0){
        // TODO: path planner timeout?
        desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1;
      }

      double mt1 = 0, mt2 = 0;
      if (run_model_this_iter) {
        float vec_desire[DESIRE_LEN] = {0};
        if (desire >= 0 && desire < DESIRE_LEN) {
          vec_desire[desire] = 1.0;
        }

        mat3 model_transform = matmul3(yuv_transform, transform);

        mt1 = millis_since_boot();

        // TODO: don't make copies!
        memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len);

        ModelDataRaw model_buf =
            model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height,
                             model_transform, NULL, vec_desire);
        mt2 = millis_since_boot();

        model_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);
        posenet_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);

        LOG("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
        last = mt1;
      }

    }
    visionbuf_free(&yuv_ion);
  }

  visionstream_destroy(&stream);

  model_free(&model);

  LOG("joining live_thread");
  err = pthread_join(live_thread_handle, NULL);
  assert(err == 0);

  return 0;
}
<commit_msg>modeld process time logging to DEBUG (#1720)<commit_after>#include <stdio.h>
#include <stdlib.h>
#include <eigen3/Eigen/Dense>

#include "common/visionbuf.h"
#include "common/visionipc.h"
#include "common/swaglog.h"

#include "models/driving.h"
#include "messaging.hpp"
volatile sig_atomic_t do_exit = 0;

static void set_do_exit(int sig) {
  do_exit = 1;
}

// globals
bool run_model;
mat3 cur_transform;
pthread_mutex_t transform_lock;

void* live_thread(void *arg) {
  int err;
  set_thread_name("live");

  SubMaster sm({"liveCalibration"});
  /*
     import numpy as np
     from common.transformations.model import medmodel_frame_from_road_frame
     medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
     ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
  */
  Eigen::Matrix<float, 3, 3> ground_from_medmodel_frame;
  ground_from_medmodel_frame <<
    0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
    -1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
    -1.84808520e-20, 9.00738606e-04,-4.28751576e-02;

  Eigen::Matrix<float, 3, 3> eon_intrinsics;
  eon_intrinsics <<
    910.0, 0.0, 582.0,
    0.0, 910.0, 437.0,
    0.0,   0.0,   1.0;

  while (!do_exit) {
    if (sm.update(10) > 0){

      auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix();
      Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
      for (int i = 0; i < 4*3; i++){
        extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
      }

      auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen;
      Eigen::Matrix<float, 3, 3> camera_frame_from_ground;
      camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
      camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
      camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3);

      auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame;

      pthread_mutex_lock(&transform_lock);
      for (int i=0; i<3*3; i++) {
        cur_transform.v[i] = warp_matrix(i / 3, i % 3);
      }

      run_model = true;
      pthread_mutex_unlock(&transform_lock);
    }
  }
  return NULL;
}

int main(int argc, char **argv) {
  int err;
  set_realtime_priority(51);

  // start calibration thread
  pthread_t live_thread_handle;
  err = pthread_create(&live_thread_handle, NULL, live_thread, NULL);
  assert(err == 0);

  // messaging
  PubMaster pm({"model", "cameraOdometry"});
  SubMaster sm({"pathPlan"});

#ifdef QCOM
  cl_device_type device_type = CL_DEVICE_TYPE_DEFAULT;
#else
  cl_device_type device_type = CL_DEVICE_TYPE_CPU;
#endif

  // cl init
  cl_device_id device_id;
  cl_context context;
  cl_command_queue q;
  {
    cl_uint num_platforms;
    err = clGetPlatformIDs(0, NULL, &num_platforms);
    assert(err == 0);

    cl_platform_id * platform_ids = new cl_platform_id[num_platforms];
    err = clGetPlatformIDs(num_platforms, platform_ids, NULL);
    assert(err == 0);

    LOGD("got %d opencl platform(s)", num_platforms);

    char cBuffer[1024];
    bool opencl_platform_found = false;

    for (size_t i = 0; i < num_platforms; i++){
      err = clGetPlatformInfo(platform_ids[i], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL);
      assert(err == 0);
      LOGD("platform[%zu] CL_PLATFORM_NAME: %s", i, cBuffer);

      cl_uint num_devices;
      err = clGetDeviceIDs(platform_ids[i], device_type, 0, NULL, &num_devices);
      if (err != 0|| !num_devices){
        continue;
      }

      // Get first device
      err = clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL);
      assert(err == 0);

      context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err);
      assert(err == 0);

      q = clCreateCommandQueue(context, device_id, 0, &err);
      assert(err == 0);

      opencl_platform_found = true;
      break;
    }

    delete[] platform_ids;

    if (!opencl_platform_found){
      LOGE("No valid openCL platform found");
      assert(opencl_platform_found);
    }

    LOGD("opencl init complete");
  }

  // init the models
  ModelState model;
  model_init(&model, device_id, context, true);
  LOGW("models loaded, modeld starting");

  // debayering does a 2x downscale
  mat3 yuv_transform = transform_scale_buffer((mat3){{
    1.0, 0.0, 0.0,
    0.0, 1.0, 0.0,
    0.0, 0.0, 1.0,
  }}, 0.5);

  // loop
  VisionStream stream;
  while (!do_exit) {
    VisionStreamBufs buf_info;
    err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info);
    if (err) {
      LOGW("visionstream connect failed");
      usleep(100000);
      continue;
    }
    LOGW("connected with buffer size: %d", buf_info.buf_len);

    // one frame in memory
    cl_mem yuv_cl;
    VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl);

    double last = 0;
    int desire = -1;
    while (!do_exit) {
      VIPCBuf *buf;
      VIPCBufExtra extra;
      buf = visionstream_get(&stream, &extra);
      if (buf == NULL) {
        LOGW("visionstream get failed");
        visionstream_destroy(&stream);
        break;
      }

      pthread_mutex_lock(&transform_lock);
      mat3 transform = cur_transform;
      const bool run_model_this_iter = run_model;
      pthread_mutex_unlock(&transform_lock);

      if (sm.update(0) > 0){
        // TODO: path planner timeout?
        desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1;
      }

      double mt1 = 0, mt2 = 0;
      if (run_model_this_iter) {
        float vec_desire[DESIRE_LEN] = {0};
        if (desire >= 0 && desire < DESIRE_LEN) {
          vec_desire[desire] = 1.0;
        }

        mat3 model_transform = matmul3(yuv_transform, transform);

        mt1 = millis_since_boot();

        // TODO: don't make copies!
        memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len);

        ModelDataRaw model_buf =
            model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height,
                             model_transform, NULL, vec_desire);
        mt2 = millis_since_boot();

        model_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);
        posenet_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);

        LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
        last = mt1;
      }

    }
    visionbuf_free(&yuv_ion);
  }

  visionstream_destroy(&stream);

  model_free(&model);

  LOG("joining live_thread");
  err = pthread_join(live_thread_handle, NULL);
  assert(err == 0);

  return 0;
}
<|endoftext|>


<commit_before>/*
 * (C) Copyright 2015 Kurento (http://kurento.org/)
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the GNU Lesser General Public License
 * (LGPL) version 2.1 which accompanies this distribution, and is available at
 * http://www.gnu.org/licenses/lgpl-2.1.html
 *
 * 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.
 *
 */

#include "ResourceManager.hpp"
#include <gst/gst.h>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <KurentoException.hpp>
#include <MediaSet.hpp>

#include <sys/resource.h>

#define GST_CAT_DEFAULT kurento_resource_manager
GST_DEBUG_CATEGORY_STATIC (GST_CAT_DEFAULT);
#define GST_DEFAULT_NAME "KurentoResourceManager"

namespace kurento
{

static int maxOpenFiles = 0;
static int maxThreads = 0;

static void
tokenize (std::string str, char sep, std::vector<std::string> &tokens)
{
  size_t start = str.find_first_not_of (sep), end;

  while (start != std::string::npos) {
    end = str.find (sep, start);
    tokens.push_back (str.substr (start, end - start) );
    start = str.find_first_not_of (sep, end);
  }
}

static int
getNumberOfThreads ()
{
  std::string stat;
  std::ifstream stat_file ("/proc/self/stat");
  std::vector <std::string> tokens;

  std::getline (stat_file, stat);
  tokenize (stat, ' ', tokens);

  stat_file.close();

  return atoi (tokens[19].c_str() );
}

static int
getMaxThreads ()
{
  if (maxThreads == 0) {
    struct rlimit limits;
    getrlimit (RLIMIT_NPROC, &limits);

    maxThreads = limits.rlim_cur;
  }

  return maxThreads;
}

static void
checkThreads (float limit_percent)
{
  int nThreads;

  nThreads = getNumberOfThreads ();

  if (nThreads > getMaxThreads () * limit_percent ) {
    throw KurentoException (NOT_ENOUGH_RESOURCES, "Too many threads");
  }
}

static int
getMaxOpenFiles ()
{
  if (maxOpenFiles == 0) {
    struct rlimit limits;
    getrlimit (RLIMIT_NOFILE, &limits);

    maxOpenFiles = limits.rlim_cur;
  }

  return maxOpenFiles;
}

static int
getNumberOfOpenFiles ()
{
  int openFiles = 0;
  DIR *d;
  struct dirent *dir;

  d = opendir ("/proc/self/fd");

  while ( (dir = readdir (d) ) != NULL) {
    openFiles ++;
  }

  closedir (d);

  return openFiles;
}

static void
checkOpenFiles (float limit_percent)
{
  int nOpenFiles;

  nOpenFiles = getNumberOfOpenFiles ();

  if (nOpenFiles > getMaxOpenFiles () * limit_percent ) {
    throw KurentoException (NOT_ENOUGH_RESOURCES, "Too many open files");
  }
}

void
checkResources (float limit_percent)
{
  checkThreads (limit_percent);
  checkOpenFiles (limit_percent);
}

void killServerOnLowResources (float limit_percent)
{
  MediaSet::getMediaSet()->signalEmptyLocked.connect ([limit_percent] () {
    GST_DEBUG ("MediaSet empty, checking resources");

    try {
      checkResources (limit_percent);
    } catch (KurentoException &e) {
      if (e.getCode() == NOT_ENOUGH_RESOURCES) {
        GST_ERROR ("Resources over the limit, server will be killed");
        kill ( getpid(), SIGTERM );
      }
    }
  });
}

} /* kurento */

static void init_debug (void) __attribute__ ( (constructor) );

static void
init_debug (void)
{
  GST_DEBUG_CATEGORY_INIT (GST_CAT_DEFAULT, GST_DEFAULT_NAME, 0,
                           GST_DEFAULT_NAME);
}
<commit_msg>ResourceManager: Improve performance<commit_after>/*
 * (C) Copyright 2015 Kurento (http://kurento.org/)
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the GNU Lesser General Public License
 * (LGPL) version 2.1 which accompanies this distribution, and is available at
 * http://www.gnu.org/licenses/lgpl-2.1.html
 *
 * 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.
 *
 */

#include "ResourceManager.hpp"
#include <gst/gst.h>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <KurentoException.hpp>
#include <MediaSet.hpp>

#include <sys/resource.h>

#define GST_CAT_DEFAULT kurento_resource_manager
GST_DEBUG_CATEGORY_STATIC (GST_CAT_DEFAULT);
#define GST_DEFAULT_NAME "KurentoResourceManager"

namespace kurento
{

static int maxOpenFiles = 0;
static int maxThreads = 0;

static int
get_int (std::string &str, char sep, int nToken)
{
  size_t start = str.find_first_not_of (sep), end;
  int count = 0;

  while (start != std::string::npos) {
    end = str.find (sep, start);

    if (count++ == nToken) {
      str[end] = '\0';
      return atoi (&str.c_str() [start]);
    }

    start = str.find_first_not_of (sep, end);
  }

  return 0;
}

static int
getNumberOfThreads ()
{
  std::string stat;
  std::ifstream stat_file ("/proc/self/stat");

  std::getline (stat_file, stat);
  stat_file.close();

  return get_int (stat, ' ', 19);
}

static int
getMaxThreads ()
{
  if (maxThreads == 0) {
    struct rlimit limits;
    getrlimit (RLIMIT_NPROC, &limits);

    maxThreads = limits.rlim_cur;
  }

  return maxThreads;
}

static void
checkThreads (float limit_percent)
{
  int nThreads;

  nThreads = getNumberOfThreads ();

  if (nThreads > getMaxThreads () * limit_percent ) {
    throw KurentoException (NOT_ENOUGH_RESOURCES, "Too many threads");
  }
}

static int
getMaxOpenFiles ()
{
  if (maxOpenFiles == 0) {
    struct rlimit limits;
    getrlimit (RLIMIT_NOFILE, &limits);

    maxOpenFiles = limits.rlim_cur;
  }

  return maxOpenFiles;
}

static int
getNumberOfOpenFiles ()
{
  int openFiles = 0;
  DIR *d;
  struct dirent *dir;

  d = opendir ("/proc/self/fd");

  while ( (dir = readdir (d) ) != NULL) {
    openFiles ++;
  }

  closedir (d);

  return openFiles;
}

static void
checkOpenFiles (float limit_percent)
{
  int nOpenFiles;

  nOpenFiles = getNumberOfOpenFiles ();

  if (nOpenFiles > getMaxOpenFiles () * limit_percent ) {
    throw KurentoException (NOT_ENOUGH_RESOURCES, "Too many open files");
  }
}

void
checkResources (float limit_percent)
{
  checkThreads (limit_percent);
  checkOpenFiles (limit_percent);
}

void killServerOnLowResources (float limit_percent)
{
  MediaSet::getMediaSet()->signalEmptyLocked.connect ([limit_percent] () {
    GST_DEBUG ("MediaSet empty, checking resources");

    try {
      checkResources (limit_percent);
    } catch (KurentoException &e) {
      if (e.getCode() == NOT_ENOUGH_RESOURCES) {
        GST_ERROR ("Resources over the limit, server will be killed");
        kill ( getpid(), SIGTERM );
      }
    }
  });
}

} /* kurento */

static void init_debug (void) __attribute__ ( (constructor) );

static void
init_debug (void)
{
  GST_DEBUG_CATEGORY_INIT (GST_CAT_DEFAULT, GST_DEFAULT_NAME, 0,
                           GST_DEFAULT_NAME);
}
<|endoftext|>


<commit_before>//#pragma optimize ("", off)
#include "geom.hh"
#include "impl.hh"
#include "shared.hh"
#include "split.hh"
#include "Geo/plane_fitting.hh"
#include "Geo/point_in_polygon.hh"
#include "Utils/circular.hh"
#include "Utils/continement_tree.hh"
#include "Utils/error_handling.hh"

#include <algorithm>

namespace Topo {

void Split<Type::EDGE>::add_point(const Info& inf_) const
{
  split_pts_.push_back(inf_);
}

void Split<Type::EDGE>::remove_duplicates()
{
  auto new_end = std::unique(split_pts_.begin(), split_pts_.end(),
    [](const Info& _a, const Info& _b) { return _a.vert_ == _b.vert_; });
  split_pts_.erase(new_end, split_pts_.end());
  if (split_pts_.size() > 1 && split_pts_.front().vert_ == split_pts_.back().vert_)
    split_pts_.pop_back();
}

bool Split<Type::EDGE>::operator()() const
{
  std::sort(split_pts_.begin(), split_pts_.end(),
    [](const Info&_a, const Info&_b) { return _a.t_ < _b.t_; });
  if (edge_->sub_type() != SubType::EDGE_REF)
    return false;
  auto ed_ref = static_cast<const EdgeRef*>(edge_.get());
  std::vector<IBase*> faces[2];
  const EE<Type::VERTEX>* vert_impl[2];
  for (size_t i = 0; i < std::size(ed_ref->verts_); ++i)
  {
    vert_impl[i] = static_cast<const EE<Type::VERTEX>*>(ed_ref->verts_[i].get());
    size_t face_nmbr = vert_impl[i]->size(Direction::Up);
    for (size_t j = 0; j < face_nmbr; ++j)
      faces[i].push_back(vert_impl[i]->get(Direction::Up, j));
    std::sort(faces[i].begin(), faces[i].end());
  }
  std::vector<IBase*> comm_faces;
  std::set_intersection(
    faces[0].begin(), faces[0].end(),
    faces[1].begin(), faces[1].end(),
    std::back_inserter(comm_faces));
  for (auto face : comm_faces)
  {
    size_t pos = SIZE_MAX;
    for (;;)
    {
      pos = face->find_child(vert_impl[0], pos);
      if (pos == SIZE_MAX)
        break;
      auto pos1 = (pos + 1) % face->size(Direction::Down);
      if (face->get(Direction::Down, pos1) == vert_impl[1])
      {
        for (auto it = split_pts_.rbegin(); it != split_pts_.rend(); ++it)
          face->insert_child(it->vert_.get(), pos1);
        continue;
      }
      pos1 = pos == 0 ? face->size(Direction::Down) - 1 : pos - 1;
      if (face->get(Direction::Down, pos1) == vert_impl[1])
      {
        for (auto it = split_pts_.begin(); it != split_pts_.end(); ++it)
          face->insert_child(it->vert_.get(), pos);
        continue;
      }
      //THROW("Impossible splt edge");
    }
  }

  return true;
}

void Split<Type::FACE>::use_face_loops(const LoopFilter& _loop_filter)
{
  Topo::Iterator<Topo::Type::FACE, Topo::Type::LOOP> fl_it(face_);
  if (fl_it.size() == 0)
    return;
  auto make_chain = [](const Topo::Wrap<Topo::Type::LOOP>& _loop,
    VertexChain& _vert_chain)
  {
    Topo::Iterator<Topo::Type::LOOP, Topo::Type::VERTEX> lv_it(_loop);
    for (auto& v : lv_it)
      _vert_chain.emplace_back(v);
  };
  auto loop_it = fl_it.begin();
  VertexChain vert_chain;
  if (_loop_filter(*loop_it))
  {
    make_chain(*loop_it, vert_chain);
    add_boundary(vert_chain);
  }
  while (++loop_it != fl_it.end())
  {
    if (_loop_filter(*loop_it))
    {
      make_chain(*loop_it, vert_chain);
      add_original_island(vert_chain);
    }
  }
}

namespace {

std::vector<Geo::Vector3> vertex_chain_to_poly(VertexChain& _chain)
{
  std::vector<Geo::Vector3> poly;
  poly.reserve(_chain.size());
  for (auto& v : _chain)
  {
    Geo::Vector3 pt;
    v->geom(pt);
    poly.push_back(pt);
  }
  return poly;
}

bool is_inside(const std::vector<Geo::Vector3>& _chain,
  VertexChain& _isle, const Geo::Vector3& _norm)
{
  Geo::Vector3 pt_inside;
  _isle[0]->geom(pt_inside);
  auto pt_clss = Geo::PointInPolygon::classify(_chain, pt_inside, &_norm);
  if (pt_clss != Geo::PointInPolygon::Inside)
    return false;
  auto int_norm = Geo::vertex_polygon_normal(_isle.begin(), _isle.end());
  if (int_norm * _norm > 0)
    std::reverse(_isle.begin(), _isle.end());
  return true;
}

} // namespace

bool Split<Type::FACE>::compute()
{
  if (boundary_chains_.empty())
    return false;
  for (auto& chain : boundary_chains_)
  {
    if (chain.empty())
      continue;
    auto poly = vertex_chain_to_poly(chain);
    std::vector<VertexChain> cur_islands;
    auto norm = Geo::vertex_polygon_normal(chain.begin(), chain.end());
    auto select_chains = [&cur_islands, &poly, &norm](
      std::vector<VertexChain>& _island_chains)
    {
      for (auto& isle : _island_chains)
      {
        if (isle.empty())
          continue;
        if (is_inside(poly, isle, norm))
          cur_islands.emplace_back(std::move(isle));
      }
    };
    select_chains(original_island_chains_);
    auto original_size = cur_islands.size();
    select_chains(island_chains_);
    bool make_loops = !cur_islands.empty();

	  auto inherit_parents = [this](Wrap<Type::FACE>& _new_face)
    {
        for (size_t i = 0; i < face_->size(Direction::Up); ++i)
        {
          auto parent = face_->get(Direction::Up, i);
          parent->insert_child(_new_face.get());
        }
    };
	  auto add_face_loop = [](
	    Wrap<Type::FACE> _new_face, VertexChain& _chain, bool _make_loops)
    {
      IBase* up_elem = _new_face.get();
      if (_make_loops)
      {
        Wrap<Type::LOOP> new_loop;
        new_loop.make<EE<Type::LOOP>>();
        _new_face->insert_child(new_loop.get());
        up_elem = new_loop.get();
      }
      else
        up_elem = _new_face.get();
      for (auto& vert : _chain)
        up_elem->insert_child(vert.get());
    };
    Wrap<Type::FACE> new_face;
    new_face.make<EE<Type::FACE>>();
    add_face_loop(new_face, chain, make_loops);
    for (auto loop : cur_islands)
    {
      add_face_loop(new_face, loop, make_loops);
      if (original_size > 0)
      {
        --original_size;
        continue;
      }
      Wrap<Type::FACE> new_int_face;
      new_int_face.make<EE<Type::FACE>>();
      std::reverse(loop.begin(), loop.end());
      add_face_loop(new_int_face, loop, false);
      inherit_parents(new_int_face);
      new_faces_.emplace_back(new_int_face);
    }
    inherit_parents(new_face);
    new_faces_.emplace_back(new_face);
  }
  face_->remove();
  return true;
}

namespace
{
void loop_split(Wrap<Type::FACE>& _face,
                Iterator<Type::FACE, Type::LOOP>& _loop_it,
                const Wrap<Type::VERTEX>& _ins_vert)
{
  auto loop = *(_loop_it.begin());
  std::vector<size_t> ins_pos;
  for (auto pos = SIZE_MAX;;)
  {
    pos = loop->find_child(_ins_vert.get(), pos);
    if (pos == SIZE_MAX)
      break;
    ins_pos.push_back(pos);
  }
  if (ins_pos.size() < 2)
    return;
  std::vector<VertexChain> chains;
  size_t last_ind = loop->size(Direction::Down) - 1;
  for (auto pos : ins_pos)
  {
    chains.emplace_back();
    for (size_t i = pos; i <= last_ind; ++i)
    {
      auto v = loop->get(Direction::Down, i);
      chains.back().emplace_back(static_cast<E<Type::VERTEX> *>(v));
    }
    last_ind = pos;
  }
  if (ins_pos.back() == 0)
    chains.front().push_back(_ins_vert);
  else if (ins_pos.back() == loop->size(Direction::Down) - 1)
    chains.back().insert(chains.front().begin(), _ins_vert);
  else
  {
    auto& first = chains.front();
    auto& last = chains.back();
    first.insert(first.begin(), last.begin(), last.end());
    chains.pop_back();
  }
  auto compare_chains = [](const VertexChain* _a, const VertexChain* _b)
  {
    int result = 0;
    auto inside = [&result](const VertexChain* _a, const VertexChain* _b, int _out)
    {
      Geo::Point pt;
      (*_a)[1]->geom(pt);
      if (PointInFace::classify((*_b), pt) != Geo::PointInPolygon::Inside)
        return false;
      result = _out;
      return true;
    };
    inside(_a, _b, -1) || inside(_b, _a, 1);
    return result;
  };
  Utils::ContainementTree<VertexChain*> cont_tree(compare_chains);
  for (auto& ch : chains)
    cont_tree.add(&ch);
  auto root = cont_tree.root();
  if (!root->next())
    return;
  Split<Type::FACE> face_splitter(_face);
  struct SelectIsles : public Split<Type::FACE>::LoopFilter
  {
    SelectIsles(const Wrap<Type::LOOP>& _loop) : loop_(_loop) {}
    bool operator()(const Topo::Wrap<Topo::Type::LOOP>& _loop) const
    {
      return loop_ != _loop;
    }
    Wrap<Type::LOOP> loop_;
  } sel_isles(loop);
  face_splitter.use_face_loops(sel_isles);
  for (; root != nullptr; root = root->next())
  {
    auto vc = root->data();
    for (auto conn = root->child(); conn != nullptr; conn = conn->next())
      vc->insert(vc->end(), conn->data()->begin(), conn->data()->end());
    face_splitter.add_boundary(*vc);
  }
  face_splitter.compute();
}

}

bool split(const Wrap<Type::VERTEX>& _ed_start,
  const Wrap<Type::VERTEX>& _ed_end, Wrap<Type::VERTEX>& _ins_vert)
{
  auto loops = shared_entities<Type::VERTEX, Type::LOOP>(_ed_start, _ed_end);
  for (auto& loop : loops)
  {
    for (auto pos = SIZE_MAX;;)
    {
      pos = loop->find_child(_ed_start.get(), pos);
      if (pos == SIZE_MAX)
        break;
      auto oth = Utils::increase(pos, loop->size(Direction::Down));
      if (loop->get(Direction::Down, oth) == _ed_end.get())
        loop->insert_child(_ins_vert.get(), pos + 1);
      oth = Utils::decrease(pos, loop->size(Direction::Down));
      if (loop->get(Direction::Down, oth) == _ed_end.get())
        loop->insert_child(_ins_vert.get(), pos);
    }
    // CHeck that theloop is aface boundary
    Iterator<Type::LOOP, Type::FACE> faces(loop);
    if (faces.size() != 1)
      continue;
    auto face = *faces.begin();
    Iterator<Type::FACE, Type::LOOP> face_loops(face);
    if (*face_loops.begin() != loop)
      continue;
    loop_split(face, face_loops, _ins_vert);
  }
  return true;
}

}
<commit_msg>Fix comment<commit_after>//#pragma optimize ("", off)
#include "geom.hh"
#include "impl.hh"
#include "shared.hh"
#include "split.hh"
#include "Geo/plane_fitting.hh"
#include "Geo/point_in_polygon.hh"
#include "Utils/circular.hh"
#include "Utils/continement_tree.hh"
#include "Utils/error_handling.hh"

#include <algorithm>

namespace Topo {

void Split<Type::EDGE>::add_point(const Info& inf_) const
{
  split_pts_.push_back(inf_);
}

void Split<Type::EDGE>::remove_duplicates()
{
  auto new_end = std::unique(split_pts_.begin(), split_pts_.end(),
    [](const Info& _a, const Info& _b) { return _a.vert_ == _b.vert_; });
  split_pts_.erase(new_end, split_pts_.end());
  if (split_pts_.size() > 1 && split_pts_.front().vert_ == split_pts_.back().vert_)
    split_pts_.pop_back();
}

bool Split<Type::EDGE>::operator()() const
{
  std::sort(split_pts_.begin(), split_pts_.end(),
    [](const Info&_a, const Info&_b) { return _a.t_ < _b.t_; });
  if (edge_->sub_type() != SubType::EDGE_REF)
    return false;
  auto ed_ref = static_cast<const EdgeRef*>(edge_.get());
  std::vector<IBase*> faces[2];
  const EE<Type::VERTEX>* vert_impl[2];
  for (size_t i = 0; i < std::size(ed_ref->verts_); ++i)
  {
    vert_impl[i] = static_cast<const EE<Type::VERTEX>*>(ed_ref->verts_[i].get());
    size_t face_nmbr = vert_impl[i]->size(Direction::Up);
    for (size_t j = 0; j < face_nmbr; ++j)
      faces[i].push_back(vert_impl[i]->get(Direction::Up, j));
    std::sort(faces[i].begin(), faces[i].end());
  }
  std::vector<IBase*> comm_faces;
  std::set_intersection(
    faces[0].begin(), faces[0].end(),
    faces[1].begin(), faces[1].end(),
    std::back_inserter(comm_faces));
  for (auto face : comm_faces)
  {
    size_t pos = SIZE_MAX;
    for (;;)
    {
      pos = face->find_child(vert_impl[0], pos);
      if (pos == SIZE_MAX)
        break;
      auto pos1 = (pos + 1) % face->size(Direction::Down);
      if (face->get(Direction::Down, pos1) == vert_impl[1])
      {
        for (auto it = split_pts_.rbegin(); it != split_pts_.rend(); ++it)
          face->insert_child(it->vert_.get(), pos1);
        continue;
      }
      pos1 = pos == 0 ? face->size(Direction::Down) - 1 : pos - 1;
      if (face->get(Direction::Down, pos1) == vert_impl[1])
      {
        for (auto it = split_pts_.begin(); it != split_pts_.end(); ++it)
          face->insert_child(it->vert_.get(), pos);
        continue;
      }
      //THROW("Impossible splt edge");
    }
  }

  return true;
}

void Split<Type::FACE>::use_face_loops(const LoopFilter& _loop_filter)
{
  Topo::Iterator<Topo::Type::FACE, Topo::Type::LOOP> fl_it(face_);
  if (fl_it.size() == 0)
    return;
  auto make_chain = [](const Topo::Wrap<Topo::Type::LOOP>& _loop,
    VertexChain& _vert_chain)
  {
    Topo::Iterator<Topo::Type::LOOP, Topo::Type::VERTEX> lv_it(_loop);
    for (auto& v : lv_it)
      _vert_chain.emplace_back(v);
  };
  auto loop_it = fl_it.begin();
  VertexChain vert_chain;
  if (_loop_filter(*loop_it))
  {
    make_chain(*loop_it, vert_chain);
    add_boundary(vert_chain);
  }
  while (++loop_it != fl_it.end())
  {
    if (_loop_filter(*loop_it))
    {
      make_chain(*loop_it, vert_chain);
      add_original_island(vert_chain);
    }
  }
}

namespace {

std::vector<Geo::Vector3> vertex_chain_to_poly(VertexChain& _chain)
{
  std::vector<Geo::Vector3> poly;
  poly.reserve(_chain.size());
  for (auto& v : _chain)
  {
    Geo::Vector3 pt;
    v->geom(pt);
    poly.push_back(pt);
  }
  return poly;
}

bool is_inside(const std::vector<Geo::Vector3>& _chain,
  VertexChain& _isle, const Geo::Vector3& _norm)
{
  Geo::Vector3 pt_inside;
  _isle[0]->geom(pt_inside);
  auto pt_clss = Geo::PointInPolygon::classify(_chain, pt_inside, &_norm);
  if (pt_clss != Geo::PointInPolygon::Inside)
    return false;
  auto int_norm = Geo::vertex_polygon_normal(_isle.begin(), _isle.end());
  if (int_norm * _norm > 0)
    std::reverse(_isle.begin(), _isle.end());
  return true;
}

} // namespace

bool Split<Type::FACE>::compute()
{
  if (boundary_chains_.empty())
    return false;
  for (auto& chain : boundary_chains_)
  {
    if (chain.empty())
      continue;
    auto poly = vertex_chain_to_poly(chain);
    std::vector<VertexChain> cur_islands;
    auto norm = Geo::vertex_polygon_normal(chain.begin(), chain.end());
    auto select_chains = [&cur_islands, &poly, &norm](
      std::vector<VertexChain>& _island_chains)
    {
      for (auto& isle : _island_chains)
      {
        if (isle.empty())
          continue;
        if (is_inside(poly, isle, norm))
          cur_islands.emplace_back(std::move(isle));
      }
    };
    select_chains(original_island_chains_);
    auto original_size = cur_islands.size();
    select_chains(island_chains_);
    bool make_loops = !cur_islands.empty();

	  auto inherit_parents = [this](Wrap<Type::FACE>& _new_face)
    {
        for (size_t i = 0; i < face_->size(Direction::Up); ++i)
        {
          auto parent = face_->get(Direction::Up, i);
          parent->insert_child(_new_face.get());
        }
    };
	  auto add_face_loop = [](
	    Wrap<Type::FACE> _new_face, VertexChain& _chain, bool _make_loops)
    {
      IBase* up_elem = _new_face.get();
      if (_make_loops)
      {
        Wrap<Type::LOOP> new_loop;
        new_loop.make<EE<Type::LOOP>>();
        _new_face->insert_child(new_loop.get());
        up_elem = new_loop.get();
      }
      else
        up_elem = _new_face.get();
      for (auto& vert : _chain)
        up_elem->insert_child(vert.get());
    };
    Wrap<Type::FACE> new_face;
    new_face.make<EE<Type::FACE>>();
    add_face_loop(new_face, chain, make_loops);
    for (auto loop : cur_islands)
    {
      add_face_loop(new_face, loop, make_loops);
      if (original_size > 0)
      {
        --original_size;
        continue;
      }
      Wrap<Type::FACE> new_int_face;
      new_int_face.make<EE<Type::FACE>>();
      std::reverse(loop.begin(), loop.end());
      add_face_loop(new_int_face, loop, false);
      inherit_parents(new_int_face);
      new_faces_.emplace_back(new_int_face);
    }
    inherit_parents(new_face);
    new_faces_.emplace_back(new_face);
  }
  face_->remove();
  return true;
}

namespace
{
void loop_split(Wrap<Type::FACE>& _face,
                Iterator<Type::FACE, Type::LOOP>& _loop_it,
                const Wrap<Type::VERTEX>& _ins_vert)
{
  auto loop = *(_loop_it.begin());
  std::vector<size_t> ins_pos;
  for (auto pos = SIZE_MAX;;)
  {
    pos = loop->find_child(_ins_vert.get(), pos);
    if (pos == SIZE_MAX)
      break;
    ins_pos.push_back(pos);
  }
  if (ins_pos.size() < 2)
    return;
  std::vector<VertexChain> chains;
  size_t last_ind = loop->size(Direction::Down) - 1;
  for (auto pos : ins_pos)
  {
    chains.emplace_back();
    for (size_t i = pos; i <= last_ind; ++i)
    {
      auto v = loop->get(Direction::Down, i);
      chains.back().emplace_back(static_cast<E<Type::VERTEX> *>(v));
    }
    last_ind = pos;
  }
  if (ins_pos.back() == 0)
    chains.front().push_back(_ins_vert);
  else if (ins_pos.back() == loop->size(Direction::Down) - 1)
    chains.back().insert(chains.front().begin(), _ins_vert);
  else
  {
    auto& first = chains.front();
    auto& last = chains.back();
    first.insert(first.begin(), last.begin(), last.end());
    chains.pop_back();
  }
  auto compare_chains = [](const VertexChain* _a, const VertexChain* _b)
  {
    int result = 0;
    auto inside = [&result](const VertexChain* _a, const VertexChain* _b, int _out)
    {
      Geo::Point pt;
      (*_a)[1]->geom(pt);
      if (PointInFace::classify((*_b), pt) != Geo::PointInPolygon::Inside)
        return false;
      result = _out;
      return true;
    };
    inside(_a, _b, -1) || inside(_b, _a, 1);
    return result;
  };
  Utils::ContainementTree<VertexChain*> cont_tree(compare_chains);
  for (auto& ch : chains)
    cont_tree.add(&ch);
  auto root = cont_tree.root();
  if (!root->next())
    return;
  Split<Type::FACE> face_splitter(_face);
  struct SelectIsles : public Split<Type::FACE>::LoopFilter
  {
    SelectIsles(const Wrap<Type::LOOP>& _loop) : loop_(_loop) {}
    bool operator()(const Topo::Wrap<Topo::Type::LOOP>& _loop) const
    {
      return loop_ != _loop;
    }
    Wrap<Type::LOOP> loop_;
  } sel_isles(loop);
  face_splitter.use_face_loops(sel_isles);
  for (; root != nullptr; root = root->next())
  {
    auto vc = root->data();
    for (auto conn = root->child(); conn != nullptr; conn = conn->next())
      vc->insert(vc->end(), conn->data()->begin(), conn->data()->end());
    face_splitter.add_boundary(*vc);
  }
  face_splitter.compute();
}

}

bool split(const Wrap<Type::VERTEX>& _ed_start,
  const Wrap<Type::VERTEX>& _ed_end, Wrap<Type::VERTEX>& _ins_vert)
{
  auto loops = shared_entities<Type::VERTEX, Type::LOOP>(_ed_start, _ed_end);
  for (auto& loop : loops)
  {
    for (auto pos = SIZE_MAX;;)
    {
      pos = loop->find_child(_ed_start.get(), pos);
      if (pos == SIZE_MAX)
        break;
      auto oth = Utils::increase(pos, loop->size(Direction::Down));
      if (loop->get(Direction::Down, oth) == _ed_end.get())
        loop->insert_child(_ins_vert.get(), pos + 1);
      oth = Utils::decrease(pos, loop->size(Direction::Down));
      if (loop->get(Direction::Down, oth) == _ed_end.get())
        loop->insert_child(_ins_vert.get(), pos);
    }
    // CHeck that the loop is aface boundary
    Iterator<Type::LOOP, Type::FACE> faces(loop);
    if (faces.size() != 1)
      continue;
    auto face = *faces.begin();
    Iterator<Type::FACE, Type::LOOP> face_loops(face);
    if (*face_loops.begin() != loop)
      continue;
    loop_split(face, face_loops, _ins_vert);
  }
  return true;
}

}
<|endoftext|>