<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    ResampleImageFilter3.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

//  Software Guide : BeginLatex
//
//  Now that the principles behind the \code{ResampleImageFilter} has been
//  layed out, let's have some fun with it !
//
//  Figure \ref{fig:ResampleImageFilterTransformComposition6} illustrates the
//  generic case of the resampling process. The origin and spacing of the
//  output image has been choosen to be different from those of the input
//  image.  The circles represent the \emph{center} of pixels. They are
//  inscribed in a rectangle representing the \emph{coverage} of this pixel.
//  The spacing specifies the distance between pixel centers along every
//  dimension.
//
//  The transform applied is a rotation of $30$ degrees. It is important to
//  note here that the transform supplied to the \code{ResampleImageFilter} is
//  a \emph{counter-clockwise} rotation. This transform rotates the
//  \emph{coordinate system} of the output image 30 degrees counter-clockwise.
//  When the two images are relocated in a common coordinate system --- as in
//  figure \ref{fig:ResampleImageFilterTransformComposition6} --- the result is
//  that the frame of the output image appears rotated 30 degrees
//  \emph{clockwise}.  If the output image is seen with its coordinate system
//  straighten up, the image content appears rotated 30 degrees
//  \emph{counter-clockwise}. Before continue reading this section, you may
//  want to meditate a bit on this fact while enjoying a cup of coffee.
//
// \begin{figure}
// \center
// \includegraphics[width=12cm]{ResampleImageFilterTransformComposition6.eps}
// \caption{Effect of selecting the origin of the output image}
// \label{fig:ResampleImageFilterTransformComposition6}
// \end{figure}
//
//  The following code implements these condition with the only difference of
//  selecting a spacing 40 times smaller and a number of pixels 40 times larger
//  in both dimensions. Without these change, few detail will be recognizable
//  on the images.  Note that the spacing and origin of the input image should
//  be prepared in advance by using other means since this filter cannot alter
//  in any way the actual content of the input image.
//
//  Software Guide : EndLatex

#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkResampleImageFilter.h"
#include "itkAffineTransform.h"
#include "itkNearestNeighborInterpolateImageFunction.h"

int main( int argc, char ** argv )
{

  if( argc < 4 )
    {
    std::cerr << "Usage: " << std::endl;
    std::cerr << argv[0] << "  inputImageFile  outputImageFile";
    std::cerr << "  [exampleAction={0,1}]" << std::endl;
    return 1;
    }

  int exampleAction = 0;

  if( argc >= 4 )
    {
    exampleAction = atoi( argv[3] );
    }

  const     unsigned int   Dimension = 2;
  typedef   unsigned char  InputPixelType;
  typedef   unsigned char  OutputPixelType;

  typedef itk::Image< InputPixelType,  Dimension >   InputImageType;
  typedef itk::Image< OutputPixelType, Dimension >   OutputImageType;

  typedef itk::ImageFileReader< InputImageType  >  ReaderType;
  typedef itk::ImageFileWriter< OutputImageType >  WriterType;

  ReaderType::Pointer reader = ReaderType::New();
  WriterType::Pointer writer = WriterType::New();

  reader->SetFileName( argv[1] );
  writer->SetFileName( argv[2] );

  typedef itk::ResampleImageFilter<
                  InputImageType, OutputImageType >  FilterType;

  FilterType::Pointer filter = FilterType::New();

  typedef itk::AffineTransform< double, Dimension >  TransformType;

  TransformType::Pointer transform = TransformType::New();

  typedef itk::NearestNeighborInterpolateImageFunction<
                       InputImageType, double >  InterpolatorType;
  InterpolatorType::Pointer interpolator = InterpolatorType::New();

  filter->SetInterpolator( interpolator );

  //  Software Guide : BeginLatex
  //
  //  In order to facilitate the interpretation of the transform we set the
  //  default pixel value to a distintly visible gray level.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->SetDefaultPixelValue( 100 );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The spacing is selected here to be 40 times smaller than the one
  //  illustrated in figure \ref{fig:ResampleImageFilterTransformComposition6}.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  double spacing[ Dimension ];
  spacing[0] = 40.0 / 40.0; // pixel spacing in millimeters along X
  spacing[1] = 30.0 / 40.0; // pixel spacing in millimeters along Y

  filter->SetOutputSpacing( spacing );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  Let us now set up the origin of the output image. Note that the values
  //  provided here will be those of the space coordinates for the pixel of
  //  index $(0,0)$.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  double origin[ Dimension ];
  origin[0] =  50.0;  // X space coordinate of origin
  origin[1] = 130.0;  // Y space coordinate of origin

  filter->SetOutputOrigin( origin );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The output image size is defined to be 40 times the one illustrated on
  //  the figure.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  InputImageType::SizeType   size;

  size[0] = 5 * 40;  // number of pixels along X
  size[1] = 4 * 40;  // number of pixels along Y

  filter->SetSize( size );
  // Software Guide : EndCodeSnippet

  filter->SetInput( reader->GetOutput() );
  writer->SetInput( filter->GetOutput() );

  //  Software Guide : BeginLatex
  //
  //  Rotations are performed around the origin of coordinates. The process of
  //  positioning the output image frame as it is shown in the figure reguires
  //  three steps. First, the image origin must be moved to the origin of the
  //  coordinate system, this is done by applying a translationof
  //  $(-50,-130.0))$. In a second step, a rotation of 30 degrees is performed.
  //  The third and final step implies to translate back the origing to the
  //  previous location, which can be done with a translation of $(50.0,130)$.
  //  Note that the AffineTransform uses a second boolean argument to specify
  //  if the current modification of the transform should be pre-composed or
  //  post-composed with the current transform content.
  //
  //  Angles are specified in Radians to the Affine transform.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  TransformType::OutputVectorType translation1;

  translation1[0] =   -50.0;
  translation1[1] =  -130.0;

  transform->Translate( translation1 );
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginCodeSnippet
  const double degreesToRadians = atan(1.0) / 45.0;
  transform->Rotate2D( 30.0 * degreesToRadians, false );

  filter->SetTransform( transform );
  // Software Guide : EndCodeSnippet

   // Software Guide : BeginCodeSnippet
  TransformType::OutputVectorType translation2;

  translation2[0] =    50.0;
  translation2[1] =   130.0;

  transform->Translate( translation2, false );
  // Software Guide : EndCodeSnippet

  if( exampleAction == 0 )
    {
      try
        {
        writer->Update();
        }
      catch( itk::ExceptionObject & excep )
        {
        std::cerr << "Exception catched !" << std::endl;
        std::cerr << excep << std::endl;
        }
    }

  return 0;

}

<commit_msg>DOC: ENH: Figure with Input and Output images added.<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    ResampleImageFilter3.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

//  Software Guide : BeginLatex
//
//  Now that the principles behind the \code{ResampleImageFilter} has been
//  layed out, let's have some fun with it !
//
//  Figure \ref{fig:ResampleImageFilterTransformComposition6} illustrates the
//  generic case of the resampling process. The origin and spacing of the
//  output image has been choosen to be different from those of the input
//  image.  The circles represent the \emph{center} of pixels. They are
//  inscribed in a rectangle representing the \emph{coverage} of this pixel.
//  The spacing specifies the distance between pixel centers along every
//  dimension.
//
//  The transform applied is a rotation of $30$ degrees. It is important to
//  note here that the transform supplied to the \code{ResampleImageFilter} is
//  a \emph{counter-clockwise} rotation. This transform rotates the
//  \emph{coordinate system} of the output image 30 degrees counter-clockwise.
//  When the two images are relocated in a common coordinate system --- as in
//  figure \ref{fig:ResampleImageFilterTransformComposition6} --- the result is
//  that the frame of the output image appears rotated 30 degrees
//  \emph{clockwise}.  If the output image is seen with its coordinate system
//  straighten up, the image content appears rotated 30 degrees
//  \emph{counter-clockwise}. Before continue reading this section, you may
//  want to meditate a bit on this fact while enjoying a cup of coffee.
//
// \begin{figure}
// \center
// \includegraphics[width=12cm]{ResampleImageFilterTransformComposition6.eps}
// \caption{Effect of selecting the origin of the output image}
// \label{fig:ResampleImageFilterTransformComposition6}
// \end{figure}
//
//  The following code implements these condition with the only difference of
//  selecting a spacing 40 times smaller and a number of pixels 40 times larger
//  in both dimensions. Without these change, few detail will be recognizable
//  on the images.  Note that the spacing and origin of the input image should
//  be prepared in advance by using other means since this filter cannot alter
//  in any way the actual content of the input image.
//
//  Software Guide : EndLatex

#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkResampleImageFilter.h"
#include "itkAffineTransform.h"
#include "itkNearestNeighborInterpolateImageFunction.h"

int main( int argc, char ** argv )
{

  if( argc < 4 )
    {
    std::cerr << "Usage: " << std::endl;
    std::cerr << argv[0] << "  inputImageFile  outputImageFile";
    std::cerr << "  [exampleAction={0,1}]" << std::endl;
    return 1;
    }

  int exampleAction = 0;

  if( argc >= 4 )
    {
    exampleAction = atoi( argv[3] );
    }

  const     unsigned int   Dimension = 2;
  typedef   unsigned char  InputPixelType;
  typedef   unsigned char  OutputPixelType;

  typedef itk::Image< InputPixelType,  Dimension >   InputImageType;
  typedef itk::Image< OutputPixelType, Dimension >   OutputImageType;

  typedef itk::ImageFileReader< InputImageType  >  ReaderType;
  typedef itk::ImageFileWriter< OutputImageType >  WriterType;

  ReaderType::Pointer reader = ReaderType::New();
  WriterType::Pointer writer = WriterType::New();

  reader->SetFileName( argv[1] );
  writer->SetFileName( argv[2] );

  typedef itk::ResampleImageFilter<
                  InputImageType, OutputImageType >  FilterType;

  FilterType::Pointer filter = FilterType::New();

  typedef itk::AffineTransform< double, Dimension >  TransformType;

  TransformType::Pointer transform = TransformType::New();

  typedef itk::NearestNeighborInterpolateImageFunction<
                       InputImageType, double >  InterpolatorType;
  InterpolatorType::Pointer interpolator = InterpolatorType::New();

  filter->SetInterpolator( interpolator );

  //  Software Guide : BeginLatex
  //
  //  In order to facilitate the interpretation of the transform we set the
  //  default pixel value to a distintly visible gray level.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  filter->SetDefaultPixelValue( 100 );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The spacing is selected here to be 40 times smaller than the one
  //  illustrated in figure \ref{fig:ResampleImageFilterTransformComposition6}.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  double spacing[ Dimension ];
  spacing[0] = 40.0 / 40.0; // pixel spacing in millimeters along X
  spacing[1] = 30.0 / 40.0; // pixel spacing in millimeters along Y

  filter->SetOutputSpacing( spacing );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  Let us now set up the origin of the output image. Note that the values
  //  provided here will be those of the space coordinates for the pixel of
  //  index $(0,0)$.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  double origin[ Dimension ];
  origin[0] =  50.0;  // X space coordinate of origin
  origin[1] = 130.0;  // Y space coordinate of origin

  filter->SetOutputOrigin( origin );
  // Software Guide : EndCodeSnippet

  //  Software Guide : BeginLatex
  //
  //  The output image size is defined to be 40 times the one illustrated on
  //  the figure.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  InputImageType::SizeType   size;

  size[0] = 5 * 40;  // number of pixels along X
  size[1] = 4 * 40;  // number of pixels along Y

  filter->SetSize( size );
  // Software Guide : EndCodeSnippet

  filter->SetInput( reader->GetOutput() );
  writer->SetInput( filter->GetOutput() );

  //  Software Guide : BeginLatex
  //
  //  Rotations are performed around the origin of coordinates. The process of
  //  positioning the output image frame as it is shown in the figure reguires
  //  three steps. First, the image origin must be moved to the origin of the
  //  coordinate system, this is done by applying a translationof
  //  $(-50,-130.0))$. In a second step, a rotation of 30 degrees is performed.
  //  The third and final step implies to translate back the origing to the
  //  previous location, which can be done with a translation of $(50.0,130)$.
  //  Note that the AffineTransform uses a second boolean argument to specify
  //  if the current modification of the transform should be pre-composed or
  //  post-composed with the current transform content.
  //
  //  Angles are specified in Radians to the Affine transform.
  //
  //  Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  TransformType::OutputVectorType translation1;

  translation1[0] =   -50.0;
  translation1[1] =  -130.0;

  transform->Translate( translation1 );
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginCodeSnippet
  const double degreesToRadians = atan(1.0) / 45.0;
  transform->Rotate2D( 30.0 * degreesToRadians, false );

  filter->SetTransform( transform );
  // Software Guide : EndCodeSnippet

   // Software Guide : BeginCodeSnippet
  TransformType::OutputVectorType translation2;

  translation2[0] =    50.0;
  translation2[1] =   130.0;

  transform->Translate( translation2, false );
  // Software Guide : EndCodeSnippet

  if( exampleAction == 0 )
    {
      try
        {
        writer->Update();
        }
      catch( itk::ExceptionObject & excep )
        {
        std::cerr << "Exception catched !" << std::endl;
        std::cerr << excep << std::endl;
        }
    }

  //  Software Guide : BeginLatex
  //
  // \begin{figure}
  // \center
  // \includegraphics[height=6cm]{ResampleImageFilterInput2x3.eps}
  // \includegraphics[height=4cm]{ResampleImageFilterOutput9.eps}
  // \caption{Effect of a rotation on the resampling filter. Input image at left), Output image at right.}
  // \label{fig:ResampleImageFilterOutput9}
  // \end{figure}
  //
  //  Figure \ref{fig:ResampleImageFilterOutput9} presents the actual input and
  //  outpu images of this example as shown by a correct viewer which takes
  //  spacing into account. Note the \emph{clockwise} versus
  //  \emph{counter-clockwise} effect discussed previously between the
  //  representation in figure
  //  \ref{fig:ResampleImageFilterTransformComposition6} and figure
  //  \ref{fig:ResampleImageFilterOutput9}.
  //
  //  Software Guide : EndLatex

  return 0;

}

<|endoftext|>


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

#include "modules/planning/planning.h"

#include <algorithm>
#include <vector>

#include "google/protobuf/repeated_field.h"

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory/trajectory_stitcher.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/reference_line/reference_line_provider.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleStateProvider;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

Status Planning::InitFrame(const uint32_t sequence_num,
                           const TrajectoryPoint& planning_start_point,
                           const VehicleState& vehicle_state) {
  frame_.reset(new Frame(sequence_num, planning_start_point, vehicle_state));
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction: " << prediction.DebugString();
  }
  auto status = frame_->Init();
  if (!status.ok()) {
    AERROR << "failed to init frame";
    return Status(ErrorCode::PLANNING_ERROR, "init frame failed");
  }
  return Status::OK();
}

Status Planning::Init() {
  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
  CHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();

  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                               &config_))
      << "failed to load planning config file " << FLAGS_planning_config_file;
  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_planning_adapter_config_filename);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingRequest() == nullptr) {
    std::string error_msg("RoutingRequest is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_prediction && AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_traffic_light &&
      AdapterManager::GetTrafficLightDetection() == nullptr) {
    std::string error_msg("Traffic Light Detection is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  ReferenceLineProvider::instance()->Init(
      hdmap_, config_.qp_spline_reference_line_smoother_config());

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

bool Planning::IsVehicleStateValid(const VehicleState& vehicle_state) {
  if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) ||
      std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) ||
      std::isnan(vehicle_state.kappa()) ||
      std::isnan(vehicle_state.linear_velocity()) ||
      std::isnan(vehicle_state.linear_acceleration())) {
    return false;
  }
  return true;
}

Status Planning::Start() {
  timer_ = AdapterManager::CreateTimer(
      ros::Duration(1.0 / FLAGS_planning_loop_rate), &Planning::OnTimer, this);
  ReferenceLineProvider::instance()->Start();
  AINFO << "Planning started";
  return Status::OK();
}

void Planning::OnTimer(const ros::TimerEvent&) { RunOnce(); }

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb,
                                 double timestamp) {
  trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  if (AdapterManager::GetRoutingResponse() &&
      !AdapterManager::GetRoutingResponse()->Empty()) {
    trajectory_pb->mutable_routing_header()->CopyFrom(
        AdapterManager::GetRoutingResponse()->GetLatestObserved().header());
  }

  // NOTICE:
  // Since we are using the time at each cycle beginning as timestamp, the
  // relative time of each trajectory point should be modified so that we can
  // use the current timestamp in header.

  // auto* trajectory_points = trajectory_pb.mutable_trajectory_point();
  if (!FLAGS_planning_test_mode) {
    const double dt = timestamp - Clock::NowInSecond();
    for (auto& p : *trajectory_pb->mutable_trajectory_point()) {
      p.set_relative_time(p.relative_time() + dt);
    }
  }
  Publish(trajectory_pb);
}

void Planning::RunOnce() {
  const double start_timestamp = Clock::NowInSecond();

  // snapshot all coming data
  AdapterManager::Observe();

  ADCTrajectory not_ready_pb;
  auto* not_ready = not_ready_pb.mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();
  if (AdapterManager::GetLocalization()->Empty()) {
    not_ready->set_reason("localization not ready");
  } else if (AdapterManager::GetChassis()->Empty()) {
    not_ready->set_reason("chassis not ready");
  } else if (AdapterManager::GetRoutingResponse()->Empty()) {
    not_ready->set_reason("routing not ready");
  }
  if (not_ready->has_reason()) {
    AERROR << not_ready->reason() << "; skip the planning cycle.";
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  Status status =
      VehicleStateProvider::instance()->Update(localization, chassis);
  VehicleState vehicle_state =
      VehicleStateProvider::instance()->vehicle_state();

  // estimate (x, y) at current timestamp
  // This estimate is only valid if the current time and vehicle state timestamp
  // differs only a small amount (20ms). When the different is too large, the
  // estimation is invalid.
  DCHECK_GT(start_timestamp, vehicle_state.timestamp());
  if (FLAGS_estimate_current_vehicle_state &&
      start_timestamp - vehicle_state.timestamp() < 0.020) {
    auto future_xy = VehicleStateProvider::instance()->EstimateFuturePosition(
        start_timestamp - vehicle_state.timestamp());
    vehicle_state.set_x(future_xy.x());
    vehicle_state.set_y(future_xy.y());
    vehicle_state.set_timestamp(start_timestamp);
  }

  if (!status.ok() || !IsVehicleStateValid(vehicle_state)) {
    AERROR << "Update VehicleStateProvider failed.";
    not_ready->set_reason("Update VehicleStateProvider failed.");
    status.Save(not_ready_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  // Update reference line provider
  if (!ReferenceLineProvider::instance()->UpdateRoutingResponse(
          AdapterManager::GetRoutingResponse()->GetLatestObserved())) {
    AERROR << "Failed to update routing in reference line provider";
    return;
  }
  ReferenceLineProvider::instance()->UpdateVehicleState(vehicle_state);

  if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
    not_ready->set_reason("prediction not ready");
    AERROR << not_ready->reason() << "; skip the planning cycle.";
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
  bool is_replan = false;
  const auto stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          vehicle_state, start_timestamp, planning_cycle_time,
          last_publishable_trajectory_.get(), &is_replan);

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
  ADCTrajectory trajectory_pb;
  if (FLAGS_enable_record_debug) {
    frame_->RecordInputDebug(trajectory_pb.mutable_debug());
  }
  trajectory_pb.mutable_latency_stats()->set_init_frame_time_ms(
      Clock::NowInSecond() - start_timestamp);
  if (!status.ok()) {
    AERROR << "Init frame failed";
    if (FLAGS_publish_estop) {
      ADCTrajectory estop;
      estop.mutable_estop();
      status.Save(estop.mutable_header()->mutable_status());
      PublishPlanningPb(&estop, start_timestamp);
    }
    if (frame_) {
      auto seq_num = frame_->SequenceNum();
      FrameHistory::instance()->Add(seq_num, std::move(frame_));
    }
    return;
  }

  status = Plan(start_timestamp, stitching_trajectory, &trajectory_pb);

  const auto time_diff_ms = (Clock::NowInSecond() - start_timestamp) * 1000;
  ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";

  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (!status.ok()) {
    status.Save(trajectory_pb.mutable_header()->mutable_status());
    AERROR << "Planning failed:" << status.ToString();
    if (FLAGS_publish_estop) {
      AERROR << "Planning failed and set estop";
      trajectory_pb.mutable_estop();
    }
  }

  trajectory_pb.set_is_replan(is_replan);
  PublishPlanningPb(&trajectory_pb, start_timestamp);
  ADEBUG << "Planning pb:" << trajectory_pb.header().DebugString();

  if (frame_) {
    auto seq_num = frame_->SequenceNum();
    FrameHistory::instance()->Add(seq_num, std::move(frame_));
  }
}

void Planning::Stop() {
  AERROR << "Planning Stop is called";
  ReferenceLineProvider::instance()->Stop();
  last_publishable_trajectory_.reset(nullptr);
  frame_.reset(nullptr);
  planner_.reset(nullptr);
}

void Planning::SetLastPublishableTrajectory(
    const ADCTrajectory& adc_trajectory) {
  last_publishable_trajectory_.reset(new PublishableTrajectory(adc_trajectory));
}

Status Planning::Plan(const double current_time_stamp,
                      const std::vector<TrajectoryPoint>& stitching_trajectory,
                      ADCTrajectory* trajectory_pb) {
  auto* ptr_debug = trajectory_pb->mutable_debug();
  if (FLAGS_enable_record_debug) {
    ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
        stitching_trajectory.back());
  }
  auto status = Status::OK();
  for (auto& reference_line_info : frame_->reference_line_info()) {
    status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                            &reference_line_info);
    if (status == Status::OK()) {
      if (FLAGS_prioritize_change_lane && reference_line_info.IsDrivable() &&
          reference_line_info.IsChangeLanePath()) {
        ADEBUG << "Found change lane line, skip other reference line";
        break;
      }
    } else {
      AERROR << "planner failed to make a driving plan for: "
             << reference_line_info.Lanes().Id();
    }
  }
  const auto* best_reference_line = frame_->FindDriveReferenceLineInfo();
  if (!best_reference_line) {
    std::string msg(
        "planner failed to make a driving plan because NO valid reference line "
        "info.");
    AERROR << msg;
    if (last_publishable_trajectory_) {
      last_publishable_trajectory_->Clear();
    }
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  ptr_debug->MergeFrom(best_reference_line->debug());
  trajectory_pb->mutable_latency_stats()->MergeFrom(
      best_reference_line->latency_stats());

  best_reference_line->ExportDecision(trajectory_pb->mutable_decision());

  // Add debug information.
  if (FLAGS_enable_record_debug) {
    auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
    reference_line->set_name("planning_reference_line");
    const auto& reference_points =
        best_reference_line->reference_line().reference_points();
    for (const auto& reference_point : reference_points) {
      auto* path_point = reference_line->add_path_point();
      path_point->set_x(reference_point.x());
      path_point->set_y(reference_point.y());
      path_point->set_theta(reference_point.heading());
      path_point->set_kappa(reference_point.kappa());
      path_point->set_dkappa(reference_point.dkappa());
    }
  }

  last_publishable_trajectory_.reset(new PublishableTrajectory(
      current_time_stamp, best_reference_line->trajectory()));

  ADEBUG << "current_time_stamp: " << std::to_string(current_time_stamp);

  last_publishable_trajectory_->PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  for (size_t i = 0; i < last_publishable_trajectory_->NumOfPoints(); ++i) {
    if (last_publishable_trajectory_->TrajectoryPointAt(i).relative_time() >
        FLAGS_trajectory_time_high_density_period) {
      break;
    }
    ADEBUG << last_publishable_trajectory_->TrajectoryPointAt(i)
                  .ShortDebugString();
  }

  last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);

  return status;
}

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

#include "modules/planning/planning.h"

#include <algorithm>
#include <vector>

#include "google/protobuf/repeated_field.h"

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory/trajectory_stitcher.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/reference_line/reference_line_provider.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleStateProvider;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

Status Planning::InitFrame(const uint32_t sequence_num,
                           const TrajectoryPoint& planning_start_point,
                           const VehicleState& vehicle_state) {
  frame_.reset(new Frame(sequence_num, planning_start_point, vehicle_state));
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction: " << prediction.DebugString();
  }
  auto status = frame_->Init();
  if (!status.ok()) {
    AERROR << "failed to init frame";
    return Status(ErrorCode::PLANNING_ERROR, "init frame failed");
  }
  return Status::OK();
}

Status Planning::Init() {
  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
  CHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();

  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                               &config_))
      << "failed to load planning config file " << FLAGS_planning_config_file;
  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_planning_adapter_config_filename);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingRequest() == nullptr) {
    std::string error_msg("RoutingRequest is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_prediction && AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_traffic_light &&
      AdapterManager::GetTrafficLightDetection() == nullptr) {
    std::string error_msg("Traffic Light Detection is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  ReferenceLineProvider::instance()->Init(
      hdmap_, config_.qp_spline_reference_line_smoother_config());

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

bool Planning::IsVehicleStateValid(const VehicleState& vehicle_state) {
  if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) ||
      std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) ||
      std::isnan(vehicle_state.kappa()) ||
      std::isnan(vehicle_state.linear_velocity()) ||
      std::isnan(vehicle_state.linear_acceleration())) {
    return false;
  }
  return true;
}

Status Planning::Start() {
  timer_ = AdapterManager::CreateTimer(
      ros::Duration(1.0 / FLAGS_planning_loop_rate), &Planning::OnTimer, this);
  ReferenceLineProvider::instance()->Start();
  AINFO << "Planning started";
  return Status::OK();
}

void Planning::OnTimer(const ros::TimerEvent&) { RunOnce(); }

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb,
                                 double timestamp) {
  trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  if (AdapterManager::GetRoutingResponse() &&
      !AdapterManager::GetRoutingResponse()->Empty()) {
    trajectory_pb->mutable_routing_header()->CopyFrom(
        AdapterManager::GetRoutingResponse()->GetLatestObserved().header());
  }

  // NOTICE:
  // Since we are using the time at each cycle beginning as timestamp, the
  // relative time of each trajectory point should be modified so that we can
  // use the current timestamp in header.

  // auto* trajectory_points = trajectory_pb.mutable_trajectory_point();
  if (!FLAGS_planning_test_mode) {
    const double dt = timestamp - Clock::NowInSecond();
    for (auto& p : *trajectory_pb->mutable_trajectory_point()) {
      p.set_relative_time(p.relative_time() + dt);
    }
  }
  Publish(trajectory_pb);
}

void Planning::RunOnce() {
  const double start_timestamp = Clock::NowInSecond();

  // snapshot all coming data
  AdapterManager::Observe();

  ADCTrajectory not_ready_pb;
  auto* not_ready = not_ready_pb.mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();
  if (AdapterManager::GetLocalization()->Empty()) {
    not_ready->set_reason("localization not ready");
  } else if (AdapterManager::GetChassis()->Empty()) {
    not_ready->set_reason("chassis not ready");
  } else if (AdapterManager::GetRoutingResponse()->Empty()) {
    not_ready->set_reason("routing not ready");
  }
  if (not_ready->has_reason()) {
    AERROR << not_ready->reason() << "; skip the planning cycle.";
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  Status status =
      VehicleStateProvider::instance()->Update(localization, chassis);
  VehicleState vehicle_state =
      VehicleStateProvider::instance()->vehicle_state();

  // estimate (x, y) at current timestamp
  // This estimate is only valid if the current time and vehicle state timestamp
  // differs only a small amount (20ms). When the different is too large, the
  // estimation is invalid.
  DCHECK_GE(start_timestamp, vehicle_state.timestamp());
  if (FLAGS_estimate_current_vehicle_state &&
      start_timestamp - vehicle_state.timestamp() < 0.020) {
    auto future_xy = VehicleStateProvider::instance()->EstimateFuturePosition(
        start_timestamp - vehicle_state.timestamp());
    vehicle_state.set_x(future_xy.x());
    vehicle_state.set_y(future_xy.y());
    vehicle_state.set_timestamp(start_timestamp);
  }

  if (!status.ok() || !IsVehicleStateValid(vehicle_state)) {
    AERROR << "Update VehicleStateProvider failed.";
    not_ready->set_reason("Update VehicleStateProvider failed.");
    status.Save(not_ready_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  // Update reference line provider
  if (!ReferenceLineProvider::instance()->UpdateRoutingResponse(
          AdapterManager::GetRoutingResponse()->GetLatestObserved())) {
    AERROR << "Failed to update routing in reference line provider";
    return;
  }
  ReferenceLineProvider::instance()->UpdateVehicleState(vehicle_state);

  if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
    not_ready->set_reason("prediction not ready");
    AERROR << not_ready->reason() << "; skip the planning cycle.";
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
  bool is_replan = false;
  const auto stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          vehicle_state, start_timestamp, planning_cycle_time,
          last_publishable_trajectory_.get(), &is_replan);

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
  ADCTrajectory trajectory_pb;
  if (FLAGS_enable_record_debug) {
    frame_->RecordInputDebug(trajectory_pb.mutable_debug());
  }
  trajectory_pb.mutable_latency_stats()->set_init_frame_time_ms(
      Clock::NowInSecond() - start_timestamp);
  if (!status.ok()) {
    AERROR << "Init frame failed";
    if (FLAGS_publish_estop) {
      ADCTrajectory estop;
      estop.mutable_estop();
      status.Save(estop.mutable_header()->mutable_status());
      PublishPlanningPb(&estop, start_timestamp);
    }
    if (frame_) {
      auto seq_num = frame_->SequenceNum();
      FrameHistory::instance()->Add(seq_num, std::move(frame_));
    }
    return;
  }

  status = Plan(start_timestamp, stitching_trajectory, &trajectory_pb);

  const auto time_diff_ms = (Clock::NowInSecond() - start_timestamp) * 1000;
  ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";

  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (!status.ok()) {
    status.Save(trajectory_pb.mutable_header()->mutable_status());
    AERROR << "Planning failed:" << status.ToString();
    if (FLAGS_publish_estop) {
      AERROR << "Planning failed and set estop";
      trajectory_pb.mutable_estop();
    }
  }

  trajectory_pb.set_is_replan(is_replan);
  PublishPlanningPb(&trajectory_pb, start_timestamp);
  ADEBUG << "Planning pb:" << trajectory_pb.header().DebugString();

  if (frame_) {
    auto seq_num = frame_->SequenceNum();
    FrameHistory::instance()->Add(seq_num, std::move(frame_));
  }
}

void Planning::Stop() {
  AERROR << "Planning Stop is called";
  ReferenceLineProvider::instance()->Stop();
  last_publishable_trajectory_.reset(nullptr);
  frame_.reset(nullptr);
  planner_.reset(nullptr);
}

void Planning::SetLastPublishableTrajectory(
    const ADCTrajectory& adc_trajectory) {
  last_publishable_trajectory_.reset(new PublishableTrajectory(adc_trajectory));
}

Status Planning::Plan(const double current_time_stamp,
                      const std::vector<TrajectoryPoint>& stitching_trajectory,
                      ADCTrajectory* trajectory_pb) {
  auto* ptr_debug = trajectory_pb->mutable_debug();
  if (FLAGS_enable_record_debug) {
    ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
        stitching_trajectory.back());
  }
  auto status = Status::OK();
  for (auto& reference_line_info : frame_->reference_line_info()) {
    status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                            &reference_line_info);
    if (status == Status::OK()) {
      if (FLAGS_prioritize_change_lane && reference_line_info.IsDrivable() &&
          reference_line_info.IsChangeLanePath()) {
        ADEBUG << "Found change lane line, skip other reference line";
        break;
      }
    } else {
      AERROR << "planner failed to make a driving plan for: "
             << reference_line_info.Lanes().Id();
    }
  }
  const auto* best_reference_line = frame_->FindDriveReferenceLineInfo();
  if (!best_reference_line) {
    std::string msg(
        "planner failed to make a driving plan because NO valid reference line "
        "info.");
    AERROR << msg;
    if (last_publishable_trajectory_) {
      last_publishable_trajectory_->Clear();
    }
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  ptr_debug->MergeFrom(best_reference_line->debug());
  trajectory_pb->mutable_latency_stats()->MergeFrom(
      best_reference_line->latency_stats());

  best_reference_line->ExportDecision(trajectory_pb->mutable_decision());

  // Add debug information.
  if (FLAGS_enable_record_debug) {
    auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
    reference_line->set_name("planning_reference_line");
    const auto& reference_points =
        best_reference_line->reference_line().reference_points();
    for (const auto& reference_point : reference_points) {
      auto* path_point = reference_line->add_path_point();
      path_point->set_x(reference_point.x());
      path_point->set_y(reference_point.y());
      path_point->set_theta(reference_point.heading());
      path_point->set_kappa(reference_point.kappa());
      path_point->set_dkappa(reference_point.dkappa());
    }
  }

  last_publishable_trajectory_.reset(new PublishableTrajectory(
      current_time_stamp, best_reference_line->trajectory()));

  ADEBUG << "current_time_stamp: " << std::to_string(current_time_stamp);

  last_publishable_trajectory_->PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  for (size_t i = 0; i < last_publishable_trajectory_->NumOfPoints(); ++i) {
    if (last_publishable_trajectory_->TrajectoryPointAt(i).relative_time() >
        FLAGS_trajectory_time_high_density_period) {
      break;
    }
    ADEBUG << last_publishable_trajectory_->TrajectoryPointAt(i)
                  .ShortDebugString();
  }

  last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);

  return status;
}

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


<commit_before>// Copyright 2015 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ports/mojo_system/channel.h"

#include <errno.h>
#include <sys/uio.h>

#include <algorithm>
#include <deque>

#include "base/bind.h"
#include "base/location.h"
#include "base/macros.h"
#include "base/memory/ref_counted.h"
#include "base/memory/scoped_ptr.h"
#include "base/message_loop/message_loop.h"
#include "base/message_loop/message_pump_libevent.h"
#include "base/synchronization/lock.h"
#include "base/task_runner.h"
#include "mojo/edk/embedder/platform_channel_utils_posix.h"
#include "mojo/edk/embedder/platform_handle_vector.h"

namespace mojo {
namespace edk {

namespace {

const size_t kMaxBatchReadCapacity = 256 * 1024;

// A view over a Channel::Message object. The write queue uses these since
// large messages may need to be sent in chunks.
class MessageView {
 public:
  // Owns |message|. |offset| indexes the first unsent byte in the message.
  MessageView(Channel::MessagePtr message, size_t offset)
      : message_(std::move(message)),
        offset_(offset),
        handles_(message_->TakeHandles()) {
    DCHECK_GT(message_->data_num_bytes(), offset_);
  }

  MessageView(MessageView&& other) { *this = std::move(other); }

  MessageView& operator=(MessageView&& other) {
    message_ = std::move(other.message_);
    offset_ = other.offset_;
    handles_ = std::move(other.handles_);
    return *this;
  }

  ~MessageView() {}

  const void* data() const {
    return static_cast<const char*>(message_->data()) + offset_;
  }

  size_t data_num_bytes() const { return message_->data_num_bytes() - offset_; }

  size_t data_offset() const { return offset_; }
  void advance_data_offset(size_t num_bytes) {
    DCHECK_GT(message_->data_num_bytes(), offset_ + num_bytes);
    offset_ += num_bytes;
  }

  ScopedPlatformHandleVectorPtr TakeHandles() { return std::move(handles_); }
  Channel::MessagePtr TakeMessage() { return std::move(message_); }

 private:
  Channel::MessagePtr message_;
  size_t offset_;
  ScopedPlatformHandleVectorPtr handles_;

  DISALLOW_COPY_AND_ASSIGN(MessageView);
};

class ChannelPosix : public Channel,
                     public base::MessageLoop::DestructionObserver,
                     public base::MessagePumpLibevent::Watcher {
 public:
  ChannelPosix(Delegate* delegate,
               ScopedPlatformHandle handle,
               scoped_refptr<base::TaskRunner> io_task_runner)
      : Channel(delegate),
        self_(this),
        handle_(std::move(handle)),
        io_task_runner_(io_task_runner) {
  }

  void Start() override {
    io_task_runner_->PostTask(
        FROM_HERE, base::Bind(&ChannelPosix::StartOnIOThread, this));
  }

  void ShutDownImpl() override {
    if (io_task_runner_->RunsTasksOnCurrentThread()) {
      ShutDownOnIOThread();
    } else {
      io_task_runner_->PostTask(
          FROM_HERE, base::Bind(&ChannelPosix::ShutDownOnIOThread, this));
    }
  }

  void Write(MessagePtr message) override {
    bool write_error = false;
    {
      base::AutoLock lock(write_lock_);
      if (reject_writes_)
        return;
      if (outgoing_messages_.empty()) {
        if (!WriteNoLock(MessageView(std::move(message), 0)))
          reject_writes_ = write_error = true;
      } else {
        outgoing_messages_.emplace_back(std::move(message), 0);
      }
    }
    if (write_error)
      OnError();
  }

  ScopedPlatformHandleVectorPtr GetReadPlatformHandlesNoLock(
      size_t num_handles) override {
    read_lock().AssertAcquired();
    if (incoming_platform_handles_.size() < num_handles)
      return nullptr;
    ScopedPlatformHandleVectorPtr handles(
        new PlatformHandleVector(num_handles));
    for (size_t i = 0; i < num_handles; ++i) {
      (*handles)[i] = incoming_platform_handles_.front();
      incoming_platform_handles_.pop_front();
    }
    return handles;
  }

 private:
  ~ChannelPosix() override {
    DCHECK(io_task_runner_->RunsTasksOnCurrentThread());
    base::MessageLoop::current()->RemoveDestructionObserver(this);
    for (auto handle : incoming_platform_handles_)
      handle.CloseIfNecessary();
  }

  void StartOnIOThread() {
    DCHECK(!read_watcher_);
    DCHECK(!write_watcher_);
    read_watcher_.reset(new base::MessagePumpLibevent::FileDescriptorWatcher);
    write_watcher_.reset(new base::MessagePumpLibevent::FileDescriptorWatcher);
    base::MessageLoopForIO::current()->WatchFileDescriptor(
        handle_.get().handle, true /* persistent */,
        base::MessageLoopForIO::WATCH_READ, read_watcher_.get(), this);
    base::MessageLoop::current()->AddDestructionObserver(this);
  }

  void WaitForWriteOnIOThread() {
    base::AutoLock lock(write_lock_);
    WaitForWriteOnIOThreadNoLock();
  }

  void WaitForWriteOnIOThreadNoLock() {
    if (pending_write_)
      return;
    if (!write_watcher_)
      return;
    if (io_task_runner_->RunsTasksOnCurrentThread()) {
      pending_write_ = true;
      base::MessageLoopForIO::current()->WatchFileDescriptor(
          handle_.get().handle, false /* persistent */,
          base::MessageLoopForIO::WATCH_WRITE, write_watcher_.get(), this);
    } else {
      io_task_runner_->PostTask(
          FROM_HERE, base::Bind(&ChannelPosix::WaitForWriteOnIOThread, this));
    }
  }

  void ShutDownOnIOThread() {
    read_watcher_.reset();
    write_watcher_.reset();
    handle_.reset();

    // May destroy the |this| if it was the last reference.
    self_ = nullptr;
  }

  // base::MessageLoop::DestructionObserver:
  void WillDestroyCurrentMessageLoop() override {
    DCHECK(io_task_runner_->RunsTasksOnCurrentThread());
    if (self_)
      ShutDownOnIOThread();
  }

  // base::MessagePumpLibevent::Watcher:
  void OnFileCanReadWithoutBlocking(int fd) override {
    CHECK_EQ(fd, handle_.get().handle);

    scoped_refptr<Channel> keep_alive(this);
    bool read_error = false;
    {
      base::AutoLock lock(read_lock());

      size_t next_read_size = 0;
      size_t buffer_capacity = 0;
      size_t total_bytes_read = 0;
      size_t bytes_read = 0;
      do {
        buffer_capacity = next_read_size;
        char* buffer = GetReadBuffer(&buffer_capacity);
        DCHECK_GT(buffer_capacity, 0u);

        ssize_t read_result = PlatformChannelRecvmsg(
            handle_.get(),
            buffer,
            buffer_capacity,
            &incoming_platform_handles_);

        if (read_result > 0) {
          bytes_read = static_cast<size_t>(read_result);
          total_bytes_read += bytes_read;
          if (!OnReadCompleteNoLock(bytes_read, &next_read_size)) {
            read_error = true;
            break;
          }
        } else if (read_result == 0 ||
                   (errno != EAGAIN && errno != EWOULDBLOCK)) {
          read_error = true;
          break;
        }
      } while (bytes_read == buffer_capacity &&
               total_bytes_read < kMaxBatchReadCapacity);
    }
    if (read_error)
      OnError();
  }

  void OnFileCanWriteWithoutBlocking(int fd) override {
    scoped_refptr<Channel> keep_alive(this);
    bool write_error = false;
    {
      base::AutoLock lock(write_lock_);
      pending_write_ = false;
      if (!FlushOutgoingMessagesNoLock())
        reject_writes_ = write_error = true;
    }
    if (write_error)
      OnError();
  }

  // Attempts to write a message directly to the channel. If the full message
  // cannot be written, it's queued and a wait is initiated to write the message
  // ASAP on the I/O thread.
  bool WriteNoLock(MessageView message_view) {
    size_t bytes_written = 0;
    do {
      message_view.advance_data_offset(bytes_written);

      iovec iov = {
        const_cast<void*>(message_view.data()),
        message_view.data_num_bytes()
      };

      ssize_t result;
      ScopedPlatformHandleVectorPtr handles = message_view.TakeHandles();
      if (handles && handles->size()) {
        // TODO: Handle lots of handles.
        result = PlatformChannelSendmsgWithHandles(
            handle_.get(), &iov, 1, handles->data(), handles->size());
        handles->clear();
      } else {
        result = PlatformChannelWritev(handle_.get(), &iov, 1);
      }

      if (result < 0) {
        if (errno != EAGAIN && errno != EWOULDBLOCK)
          return false;
        outgoing_messages_.emplace_back(std::move(message_view));
        WaitForWriteOnIOThreadNoLock();
        return true;
      }

      bytes_written = static_cast<size_t>(result);
    } while (bytes_written < message_view.data_num_bytes());

    return true;
  }

  bool FlushOutgoingMessagesNoLock() {
    std::deque<MessageView> messages;
    std::swap(outgoing_messages_, messages);

    while (!messages.empty()) {
      if (!WriteNoLock(std::move(messages.front())))
        return false;

      messages.pop_front();
      if (!outgoing_messages_.empty()) {
        // The message was requeued by WriteNoLock(), so we have to wait for
        // pipe to become writable again. Repopulate the message queue and exit.
        DCHECK_EQ(outgoing_messages_.size(), 1u);
        MessageView message_view = std::move(outgoing_messages_.front());
        std::swap(messages, outgoing_messages_);
        outgoing_messages_.push_front(std::move(message_view));
        return true;
      }
    }

    return true;
  }

  // Keeps the Channel alive at least until explicit shutdown on the IO thread.
  scoped_refptr<Channel> self_;

  ScopedPlatformHandle handle_;
  scoped_refptr<base::TaskRunner> io_task_runner_;

  // These watchers must only be accessed on the IO thread.
  scoped_ptr<base::MessagePumpLibevent::FileDescriptorWatcher> read_watcher_;
  scoped_ptr<base::MessagePumpLibevent::FileDescriptorWatcher> write_watcher_;

  std::deque<PlatformHandle> incoming_platform_handles_;

  // Protects |pending_write_| and |outgoing_messages_|.
  base::Lock write_lock_;
  bool pending_write_ = false;
  bool reject_writes_ = false;
  std::deque<MessageView> outgoing_messages_;

  DISALLOW_COPY_AND_ASSIGN(ChannelPosix);
};

}  // namespace

// static
scoped_refptr<Channel> Channel::Create(
    Delegate* delegate,
    ScopedPlatformHandle platform_handle,
    scoped_refptr<base::TaskRunner> io_task_runner) {
  return new ChannelPosix(delegate, std::move(platform_handle), io_task_runner);
}

}  // namespace edk
}  // namespace mojo
<commit_msg>don't use writev/sendmsg unless sending handles<commit_after>// Copyright 2015 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ports/mojo_system/channel.h"

#include <errno.h>
#include <sys/uio.h>

#include <algorithm>
#include <deque>

#include "base/bind.h"
#include "base/location.h"
#include "base/macros.h"
#include "base/memory/ref_counted.h"
#include "base/memory/scoped_ptr.h"
#include "base/message_loop/message_loop.h"
#include "base/message_loop/message_pump_libevent.h"
#include "base/synchronization/lock.h"
#include "base/task_runner.h"
#include "mojo/edk/embedder/platform_channel_utils_posix.h"
#include "mojo/edk/embedder/platform_handle_vector.h"

namespace mojo {
namespace edk {

namespace {

const size_t kMaxBatchReadCapacity = 256 * 1024;

// A view over a Channel::Message object. The write queue uses these since
// large messages may need to be sent in chunks.
class MessageView {
 public:
  // Owns |message|. |offset| indexes the first unsent byte in the message.
  MessageView(Channel::MessagePtr message, size_t offset)
      : message_(std::move(message)),
        offset_(offset),
        handles_(message_->TakeHandles()) {
    DCHECK_GT(message_->data_num_bytes(), offset_);
  }

  MessageView(MessageView&& other) { *this = std::move(other); }

  MessageView& operator=(MessageView&& other) {
    message_ = std::move(other.message_);
    offset_ = other.offset_;
    handles_ = std::move(other.handles_);
    return *this;
  }

  ~MessageView() {}

  const void* data() const {
    return static_cast<const char*>(message_->data()) + offset_;
  }

  size_t data_num_bytes() const { return message_->data_num_bytes() - offset_; }

  size_t data_offset() const { return offset_; }
  void advance_data_offset(size_t num_bytes) {
    DCHECK_GT(message_->data_num_bytes(), offset_ + num_bytes);
    offset_ += num_bytes;
  }

  ScopedPlatformHandleVectorPtr TakeHandles() { return std::move(handles_); }
  Channel::MessagePtr TakeMessage() { return std::move(message_); }

 private:
  Channel::MessagePtr message_;
  size_t offset_;
  ScopedPlatformHandleVectorPtr handles_;

  DISALLOW_COPY_AND_ASSIGN(MessageView);
};

class ChannelPosix : public Channel,
                     public base::MessageLoop::DestructionObserver,
                     public base::MessagePumpLibevent::Watcher {
 public:
  ChannelPosix(Delegate* delegate,
               ScopedPlatformHandle handle,
               scoped_refptr<base::TaskRunner> io_task_runner)
      : Channel(delegate),
        self_(this),
        handle_(std::move(handle)),
        io_task_runner_(io_task_runner) {
  }

  void Start() override {
    io_task_runner_->PostTask(
        FROM_HERE, base::Bind(&ChannelPosix::StartOnIOThread, this));
  }

  void ShutDownImpl() override {
    if (io_task_runner_->RunsTasksOnCurrentThread()) {
      ShutDownOnIOThread();
    } else {
      io_task_runner_->PostTask(
          FROM_HERE, base::Bind(&ChannelPosix::ShutDownOnIOThread, this));
    }
  }

  void Write(MessagePtr message) override {
    bool write_error = false;
    {
      base::AutoLock lock(write_lock_);
      if (reject_writes_)
        return;
      if (outgoing_messages_.empty()) {
        if (!WriteNoLock(MessageView(std::move(message), 0)))
          reject_writes_ = write_error = true;
      } else {
        outgoing_messages_.emplace_back(std::move(message), 0);
      }
    }
    if (write_error)
      OnError();
  }

  ScopedPlatformHandleVectorPtr GetReadPlatformHandlesNoLock(
      size_t num_handles) override {
    read_lock().AssertAcquired();
    if (incoming_platform_handles_.size() < num_handles)
      return nullptr;
    ScopedPlatformHandleVectorPtr handles(
        new PlatformHandleVector(num_handles));
    for (size_t i = 0; i < num_handles; ++i) {
      (*handles)[i] = incoming_platform_handles_.front();
      incoming_platform_handles_.pop_front();
    }
    return handles;
  }

 private:
  ~ChannelPosix() override {
    DCHECK(io_task_runner_->RunsTasksOnCurrentThread());
    base::MessageLoop::current()->RemoveDestructionObserver(this);
    for (auto handle : incoming_platform_handles_)
      handle.CloseIfNecessary();
  }

  void StartOnIOThread() {
    DCHECK(!read_watcher_);
    DCHECK(!write_watcher_);
    read_watcher_.reset(new base::MessagePumpLibevent::FileDescriptorWatcher);
    write_watcher_.reset(new base::MessagePumpLibevent::FileDescriptorWatcher);
    base::MessageLoopForIO::current()->WatchFileDescriptor(
        handle_.get().handle, true /* persistent */,
        base::MessageLoopForIO::WATCH_READ, read_watcher_.get(), this);
    base::MessageLoop::current()->AddDestructionObserver(this);
  }

  void WaitForWriteOnIOThread() {
    base::AutoLock lock(write_lock_);
    WaitForWriteOnIOThreadNoLock();
  }

  void WaitForWriteOnIOThreadNoLock() {
    if (pending_write_)
      return;
    if (!write_watcher_)
      return;
    if (io_task_runner_->RunsTasksOnCurrentThread()) {
      pending_write_ = true;
      base::MessageLoopForIO::current()->WatchFileDescriptor(
          handle_.get().handle, false /* persistent */,
          base::MessageLoopForIO::WATCH_WRITE, write_watcher_.get(), this);
    } else {
      io_task_runner_->PostTask(
          FROM_HERE, base::Bind(&ChannelPosix::WaitForWriteOnIOThread, this));
    }
  }

  void ShutDownOnIOThread() {
    read_watcher_.reset();
    write_watcher_.reset();
    handle_.reset();

    // May destroy the |this| if it was the last reference.
    self_ = nullptr;
  }

  // base::MessageLoop::DestructionObserver:
  void WillDestroyCurrentMessageLoop() override {
    DCHECK(io_task_runner_->RunsTasksOnCurrentThread());
    if (self_)
      ShutDownOnIOThread();
  }

  // base::MessagePumpLibevent::Watcher:
  void OnFileCanReadWithoutBlocking(int fd) override {
    CHECK_EQ(fd, handle_.get().handle);

    scoped_refptr<Channel> keep_alive(this);
    bool read_error = false;
    {
      base::AutoLock lock(read_lock());

      size_t next_read_size = 0;
      size_t buffer_capacity = 0;
      size_t total_bytes_read = 0;
      size_t bytes_read = 0;
      do {
        buffer_capacity = next_read_size;
        char* buffer = GetReadBuffer(&buffer_capacity);
        DCHECK_GT(buffer_capacity, 0u);

        ssize_t read_result = PlatformChannelRecvmsg(
            handle_.get(),
            buffer,
            buffer_capacity,
            &incoming_platform_handles_);

        if (read_result > 0) {
          bytes_read = static_cast<size_t>(read_result);
          total_bytes_read += bytes_read;
          if (!OnReadCompleteNoLock(bytes_read, &next_read_size)) {
            read_error = true;
            break;
          }
        } else if (read_result == 0 ||
                   (errno != EAGAIN && errno != EWOULDBLOCK)) {
          read_error = true;
          break;
        }
      } while (bytes_read == buffer_capacity &&
               total_bytes_read < kMaxBatchReadCapacity);
    }
    if (read_error)
      OnError();
  }

  void OnFileCanWriteWithoutBlocking(int fd) override {
    scoped_refptr<Channel> keep_alive(this);
    bool write_error = false;
    {
      base::AutoLock lock(write_lock_);
      pending_write_ = false;
      if (!FlushOutgoingMessagesNoLock())
        reject_writes_ = write_error = true;
    }
    if (write_error)
      OnError();
  }

  // Attempts to write a message directly to the channel. If the full message
  // cannot be written, it's queued and a wait is initiated to write the message
  // ASAP on the I/O thread.
  bool WriteNoLock(MessageView message_view) {
    size_t bytes_written = 0;
    do {
      message_view.advance_data_offset(bytes_written);

      ssize_t result;
      ScopedPlatformHandleVectorPtr handles = message_view.TakeHandles();
      if (handles && handles->size()) {
        iovec iov = {
          const_cast<void*>(message_view.data()),
          message_view.data_num_bytes()
        };
        // TODO: Handle lots of handles.
        result = PlatformChannelSendmsgWithHandles(
            handle_.get(), &iov, 1, handles->data(), handles->size());
        handles->clear();
      } else {
        result = PlatformChannelWrite(handle_.get(), message_view.data(),
                                      message_view.data_num_bytes());
      }

      if (result < 0) {
        if (errno != EAGAIN && errno != EWOULDBLOCK)
          return false;
        outgoing_messages_.emplace_back(std::move(message_view));
        WaitForWriteOnIOThreadNoLock();
        return true;
      }

      bytes_written = static_cast<size_t>(result);
    } while (bytes_written < message_view.data_num_bytes());

    return true;
  }

  bool FlushOutgoingMessagesNoLock() {
    std::deque<MessageView> messages;
    std::swap(outgoing_messages_, messages);

    while (!messages.empty()) {
      if (!WriteNoLock(std::move(messages.front())))
        return false;

      messages.pop_front();
      if (!outgoing_messages_.empty()) {
        // The message was requeued by WriteNoLock(), so we have to wait for
        // pipe to become writable again. Repopulate the message queue and exit.
        DCHECK_EQ(outgoing_messages_.size(), 1u);
        MessageView message_view = std::move(outgoing_messages_.front());
        std::swap(messages, outgoing_messages_);
        outgoing_messages_.push_front(std::move(message_view));
        return true;
      }
    }

    return true;
  }

  // Keeps the Channel alive at least until explicit shutdown on the IO thread.
  scoped_refptr<Channel> self_;

  ScopedPlatformHandle handle_;
  scoped_refptr<base::TaskRunner> io_task_runner_;

  // These watchers must only be accessed on the IO thread.
  scoped_ptr<base::MessagePumpLibevent::FileDescriptorWatcher> read_watcher_;
  scoped_ptr<base::MessagePumpLibevent::FileDescriptorWatcher> write_watcher_;

  std::deque<PlatformHandle> incoming_platform_handles_;

  // Protects |pending_write_| and |outgoing_messages_|.
  base::Lock write_lock_;
  bool pending_write_ = false;
  bool reject_writes_ = false;
  std::deque<MessageView> outgoing_messages_;

  DISALLOW_COPY_AND_ASSIGN(ChannelPosix);
};

}  // namespace

// static
scoped_refptr<Channel> Channel::Create(
    Delegate* delegate,
    ScopedPlatformHandle platform_handle,
    scoped_refptr<base::TaskRunner> io_task_runner) {
  return new ChannelPosix(delegate, std::move(platform_handle), io_task_runner);
}

}  // namespace edk
}  // namespace mojo
<|endoftext|>


<commit_before>#include "tool.h"

using namespace fei;

void ToolScene::init()
{
	_camera.setCameraSize(Vec2(1000.0f, 600.0f));
	_camera.setCameraType(Camera::Type::PERSPECTIVE);
	_camera.setCameraZPos(1000.0f);
	_uiCamera.setCameraSize(Vec2(1000.0f, 600.0f));

	_uiLayer.setCamera(&_uiCamera);
	_planeLayer.setCamera(&_camera);

	_backRect.setSize(Vec2(2000.0f));
	_backRect.setCenter(Vec2::ZERO);
	_rect.setSize(Vec2(300.0f));
	_rect.setCenter(Vec2::ZERO);
	_backPlane.setShape(&_backRect);
	_plane.setShape(&_rect);
	_planeLayer.add(&_backPlane);
	_planeLayer.add(&_plane);

	for (int i = 0; i < 13; i++) {
		auto y = -300.0f + i * 50.0f;
		auto seg = new Segment(Vec2(-500.0f, y), Vec2(500.0f, y));
		auto obj = new ShapeObj();
		obj->setShape(seg);
		obj->setColor(Color::Black);
		obj->setZPos(0.01f);
		_planeLayer.add(obj);
	}
	for (int i = 0; i < 21; i++) {
		auto x = -500.0f + i * 50.0f;
		auto seg = new Segment(Vec2(x, -300.0f), Vec2(x, 300.0f));
		auto obj = new ShapeObj();
		obj->setShape(seg);
		obj->setColor(Color::Black);
		obj->setZPos(0.01f);
		_planeLayer.add(obj);
	}

	_planeLayer.setShader(NormalTool::getShader());

	_eulerLabel.setPosition(Vec2(260.0f, 260.0f));
	_normalLabel.setPosition(Vec2(260.0f, 210.0f));
	_colorButton.setPosition(Vec2(250.0f, 100.0f));

	_colorButton.setRectSize(Vec2(200.0f, 50.0f));

	_uiLayer.add(&_eulerLabel);
	_uiLayer.add(&_normalLabel);
	_uiLayer.add(&_colorButton);

	add(&_planeLayer);
	add(&_uiLayer);
}

void ToolScene::update()
{
	auto window = fei::Interface::getInstance()->getCurrentWindow();
	auto deltaV = window->getRHCursorDeltaV();
	if (window->getMouseButton(GLFW_MOUSE_BUTTON_LEFT)) {
		rotatePlane(deltaV * 0.005f);
	}
	NormalTool::getShader()->setViewCoord(Vec3(_camera.getPosition(), _camera.getCameraZPos()));
	NormalTool::getShader()->setLightData();

	double x = _plane.getAngleX();
	double y = _plane.getAngleY();
	_eulerLabel.setString(*NormalTool::getFont(), strFormat("%4.0f %4.0f", R2D(x), R2D(y)));
	double t = x + pi * 0.5;
	double ay = std::cos(t);
	double ax = (std::sin(t) * std::sin(t) * (2.0 - 2.0 * std::sin(y) + ay * ay) - 2.0) * -0.5;
	ax = std::sin(t) * std::sin(y);
	double az = 1.0 - ax * ax - ay * ay;
	if (az <= 0.0) {
		az = 0.0;
	} else {
		az = std::sqrt(az);
	}
	_normalLabel.setString(*NormalTool::getFont(), strFormat("%4.2f %4.2f %4.2f", ax, ay, az));
	Vec3 color = (Vec3(-ax, ay, az) + Vec3::ONE) * 0.5f;
	_colorButton.setBackColor(color);
	int i = static_cast<int>(color.x * 255.0f);
	i <<= 8;
	i += static_cast<int>(color.y * 255.0f);
	i <<= 8;
	i += static_cast<int>(color.z * 255.0f);
	auto str = strFormat("%06X", i);
	_colorButton.setLabelString(*NormalTool::getFont(), str);
	_colorButton.setName(str);
}

void ToolScene::resetPlane()
{
	_plane.setAngleX(0.0f);
	_plane.setAngleY(0.0f);
}

void ToolScene::rotatePlane(const Vec2& v)
{
	float nextAngleX = _plane.getAngleX() - v.y;
	float nextAngleY = _plane.getAngleY() + v.x;
	float limit = pif * 0.5f;
	if (nextAngleX >= -limit && nextAngleX <= limit) {
		_plane.rotateX(-v.y);
	}
	if (nextAngleY >= -limit && nextAngleY <= limit) {
		_plane.rotateY(v.x);
	}
	auto t = Vec2(_plane.getAngleX(), _plane.getAngleY());
	t.x = R2Df(t.x);
	t.y = R2Df(t.y);
	t.round();
	_plane.setAngleX(D2Rf(t.x));
	_plane.setAngleY(D2Rf(t.y));
}

void ToolScene::keyCallback(int key, int scancode, int action, int mods)
{
	if (action != GLFW_RELEASE) {
		float degree = 1.0f;
		if (mods == GLFW_MOD_SHIFT) {
			degree = 2.0f;
		}
		switch (key) {
		case GLFW_KEY_W:
			rotatePlane(Vec2(0.0f, D2Rf(degree)));
			break;
		case GLFW_KEY_S:
			rotatePlane(Vec2(0.0f, D2Rf(-degree)));
			break;
		case GLFW_KEY_A:
			rotatePlane(Vec2(D2Rf(-degree), 0.0f));
			break;
		case GLFW_KEY_D:
			rotatePlane(Vec2(D2Rf(degree), 0.0f));
			break;
		case GLFW_KEY_R:
			resetPlane();
			break;
		}
	}
}
<commit_msg>update NormalEditor<commit_after>#include "tool.h"

using namespace fei;

void ToolScene::init()
{
	_camera.setCameraSize(Vec2(1000.0f, 600.0f));
	_camera.setCameraType(Camera::Type::PERSPECTIVE);
	_camera.setCameraZPos(1000.0f);
	_uiCamera.setCameraSize(Vec2(1000.0f, 600.0f));

	_uiLayer.setCamera(&_uiCamera);
	_planeLayer.setCamera(&_camera);

	_backRect.setSize(Vec2(2000.0f));
	_backRect.setCenter(Vec2::ZERO);
	_rect.setSize(Vec2(300.0f));
	_rect.setCenter(Vec2::ZERO);
	_backPlane.setShape(&_backRect);
	_plane.setShape(&_rect);
	_planeLayer.add(&_backPlane);
	_planeLayer.add(&_plane);

	for (int i = 0; i < 13; i++) {
		auto y = -300.0f + i * 50.0f;
		auto seg = new Segment(Vec2(-500.0f, y), Vec2(500.0f, y));
		auto obj = new ShapeObj();
		obj->setShape(seg);
		obj->setColor(Color::Black);
		obj->setZPos(0.01f);
		_planeLayer.add(obj);
	}
	for (int i = 0; i < 21; i++) {
		auto x = -500.0f + i * 50.0f;
		auto seg = new Segment(Vec2(x, -300.0f), Vec2(x, 300.0f));
		auto obj = new ShapeObj();
		obj->setShape(seg);
		obj->setColor(Color::Black);
		obj->setZPos(0.01f);
		_planeLayer.add(obj);
	}

	_planeLayer.setShader(NormalTool::getShader());

	_eulerLabel.setPosition(Vec2(260.0f, 260.0f));
	_normalLabel.setPosition(Vec2(260.0f, 210.0f));
	_colorButton.setPosition(Vec2(250.0f, 100.0f));

	_colorButton.setRectSize(Vec2(200.0f, 50.0f));

	_uiLayer.add(&_eulerLabel);
	_uiLayer.add(&_normalLabel);
	_uiLayer.add(&_colorButton);

	add(&_planeLayer);
	add(&_uiLayer);
}

void ToolScene::update()
{
	auto window = fei::Interface::getInstance()->getCurrentWindow();
	auto deltaV = window->getRHCursorDeltaV();
	if (window->getMouseButton(GLFW_MOUSE_BUTTON_LEFT)) {
		rotatePlane(deltaV * 0.005f);
	}
	NormalTool::getShader()->setViewCoord(Vec3(_camera.getPosition(), _camera.getCameraZPos()));
	NormalTool::getShader()->setLightData();

	double x = _plane.getAngleX();
	double y = _plane.getAngleY();
	_eulerLabel.setString(*NormalTool::getFont(), strFormat("%4.0f %4.0f", R2D(x), R2D(y)));
	double t = x + pi * 0.5;
	double ay = std::cos(t);
	double ax = (std::sin(t) * std::sin(t) * (2.0 - 2.0 * std::sin(y) + ay * ay) - 2.0) * -0.5;
	ax = std::sin(t) * std::sin(y);
	double az = 1.0 - ax * ax - ay * ay;
	if (az <= 0.0) {
		az = 0.0;
	} else {
		az = std::sqrt(az);
	}
	_normalLabel.setString(*NormalTool::getFont(), strFormat("%4.2f %4.2f %4.2f", ax, ay, az));
	Vec3 color = (Vec3(-static_cast<float>(ax), static_cast<float>(ay), static_cast<float>(az)) + Vec3::ONE) * 0.5f;
	_colorButton.setBackColor(color);
	int i = static_cast<int>(color.x * 255.0f);
	i <<= 8;
	i += static_cast<int>(color.y * 255.0f);
	i <<= 8;
	i += static_cast<int>(color.z * 255.0f);
	auto str = strFormat("%06X", i);
	_colorButton.setLabelString(*NormalTool::getFont(), str);
	_colorButton.setName(str);
}

void ToolScene::resetPlane()
{
	_plane.setAngleX(0.0f);
	_plane.setAngleY(0.0f);
}

void ToolScene::rotatePlane(const Vec2& v)
{
	float nextAngleX = _plane.getAngleX() - v.y;
	float nextAngleY = _plane.getAngleY() + v.x;
	float limit = pif * 0.5f;
	if (nextAngleX >= -limit && nextAngleX <= limit) {
		_plane.rotateX(-v.y);
	} else {
		_plane.setAngleX(-_plane.getAngleX());
	}
	if (nextAngleY >= -limit && nextAngleY <= limit) {
		_plane.rotateY(v.x);
	}
	auto t = Vec2(_plane.getAngleX(), _plane.getAngleY());
	t.x = R2Df(t.x);
	t.y = R2Df(t.y);
	t.round();
	_plane.setAngleX(D2Rf(t.x));
	_plane.setAngleY(D2Rf(t.y));
}

void ToolScene::keyCallback(int key, int scancode, int action, int mods)
{
	if (action != GLFW_RELEASE) {
		float degree = 1.0f;
		if (mods == GLFW_MOD_SHIFT) {
			degree = 2.0f;
		}
		switch (key) {
		case GLFW_KEY_W:
			rotatePlane(Vec2(0.0f, D2Rf(degree)));
			break;
		case GLFW_KEY_S:
			rotatePlane(Vec2(0.0f, D2Rf(-degree)));
			break;
		case GLFW_KEY_A:
			rotatePlane(Vec2(D2Rf(-degree), 0.0f));
			break;
		case GLFW_KEY_D:
			rotatePlane(Vec2(D2Rf(degree), 0.0f));
			break;
		case GLFW_KEY_R:
			resetPlane();
			break;
		}
	}
}
<|endoftext|>


<commit_before>// Ouzel by Elviss Strazdins

#include "TextRenderer.hpp"
#include "Camera.hpp"
#include "../core/Engine.hpp"
#include "../graphics/Graphics.hpp"
#include "../assets/Cache.hpp"
#include "../utils/Utils.hpp"

namespace ouzel::scene
{
    TextRenderer::TextRenderer(const std::string& fontFile,
                               float initFontSize,
                               const std::string& initText,
                               math::Color initColor,
                               const math::Vector<float, 2>& initTextAnchor):
        shader{engine->getCache().getShader(shaderTexture)},
        blendState{engine->getCache().getBlendState(blendAlpha)},
        indexBuffer{engine->getGraphics(),
                    graphics::BufferType::index,
                    graphics::Flags::dynamic},
        vertexBuffer{engine->getGraphics(),
                     graphics::BufferType::vertex,
                     graphics::Flags::dynamic},
        text{initText},
        fontSize{initFontSize},
        textAnchor{initTextAnchor},
        color{initColor}
    {
        whitePixelTexture = engine->getCache().getTexture(textureWhitePixel);

        font = engine->getCache().getFont(fontFile);

        updateText();
    }

    TextRenderer::TextRenderer(const gui::Font& initFont,
                               float initFontSize,
                               const std::string& initText,
                               math::Color initColor,
                               const math::Vector<float, 2>& initTextAnchor):
        shader{engine->getCache().getShader(shaderTexture)},
        blendState{engine->getCache().getBlendState(blendAlpha)},
        indexBuffer{engine->getGraphics(),
                    graphics::BufferType::index,
                    graphics::Flags::dynamic},
        vertexBuffer{engine->getGraphics(),
                     graphics::BufferType::vertex,
                     graphics::Flags::dynamic},
        font{&initFont},
        text{initText},
        fontSize{initFontSize},
        textAnchor{initTextAnchor},
        color{initColor}
    {
        whitePixelTexture = engine->getCache().getTexture(textureWhitePixel);

        updateText();
    }

    void TextRenderer::setFont(const std::string& fontFile)
    {
        font = engine->getCache().getFont(fontFile);

        updateText();
    }

    void TextRenderer::setTextAnchor(const math::Vector<float, 2>& newTextAnchor)
    {
        textAnchor = newTextAnchor;

        updateText();
    }

    void TextRenderer::setFontSize(float newFontSize)
    {
        fontSize = newFontSize;

        updateText();
    }

    void TextRenderer::draw(const math::Matrix<float, 4>& transformMatrix,
                            float opacity,
                            const math::Matrix<float, 4>& renderViewProjection,
                            bool wireframe)
    {
        Component::draw(transformMatrix,
                        opacity,
                        renderViewProjection,
                        wireframe);

        if (needsMeshUpdate)
        {
            indexBuffer.setData(indices.data(), static_cast<std::uint32_t>(getVectorSize(indices)));
            vertexBuffer.setData(vertices.data(), static_cast<std::uint32_t>(getVectorSize(vertices)));

            needsMeshUpdate = false;
        }

        const auto modelViewProj = renderViewProjection * transformMatrix;
        const auto colorVector = {
            color.normR(),
            color.normG(),
            color.normB(),
            color.normA() * opacity
        };

        std::vector<std::vector<float>> fragmentShaderConstants(1);
        fragmentShaderConstants[0] = {std::begin(colorVector), std::end(colorVector)};

        std::vector<std::vector<float>> vertexShaderConstants(1);
        vertexShaderConstants[0] = {std::begin(modelViewProj.m.v), std::end(modelViewProj.m.v)};

        engine->getGraphics().setPipelineState(blendState->getResource(),
                                               shader->getResource(),
                                               graphics::CullMode::none,
                                               wireframe ? graphics::FillMode::wireframe : graphics::FillMode::solid);
        engine->getGraphics().setShaderConstants(fragmentShaderConstants,
                                                 vertexShaderConstants);
        engine->getGraphics().setTextures({wireframe ? whitePixelTexture->getResource() : texture ? texture->getResource() : 0U});
        engine->getGraphics().draw(indexBuffer.getResource(),
                                   static_cast<std::uint32_t>(indices.size()),
                                   sizeof(std::uint16_t),
                                   vertexBuffer.getResource(),
                                   graphics::DrawMode::triangleList,
                                   0);
    }

    void TextRenderer::setText(const std::string& newText)
    {
        text = newText;

        updateText();
    }

    void TextRenderer::updateText()
    {
        reset(boundingBox);

        if (font)
        {
            std::tie(indices, vertices, texture) = font->getRenderData(text, math::whiteColor, fontSize, textAnchor);
            needsMeshUpdate = true;

            for (const auto& vertex : vertices)
                insertPoint(boundingBox, vertex.position);
        }
        else
        {
            indices.clear();
            vertices.clear();
            texture.reset();
        }
    }
}
<commit_msg>Initialize whitePixelTexture and font in the initializer list<commit_after>// Ouzel by Elviss Strazdins

#include "TextRenderer.hpp"
#include "Camera.hpp"
#include "../core/Engine.hpp"
#include "../graphics/Graphics.hpp"
#include "../assets/Cache.hpp"
#include "../utils/Utils.hpp"

namespace ouzel::scene
{
    TextRenderer::TextRenderer(const std::string& fontFile,
                               float initFontSize,
                               const std::string& initText,
                               math::Color initColor,
                               const math::Vector<float, 2>& initTextAnchor):
        shader{engine->getCache().getShader(shaderTexture)},
        blendState{engine->getCache().getBlendState(blendAlpha)},
        indexBuffer{engine->getGraphics(),
                    graphics::BufferType::index,
                    graphics::Flags::dynamic},
        vertexBuffer{engine->getGraphics(),
                     graphics::BufferType::vertex,
                     graphics::Flags::dynamic},
        whitePixelTexture{engine->getCache().getTexture(textureWhitePixel)},
        font{engine->getCache().getFont(fontFile)},
        text{initText},
        fontSize{initFontSize},
        textAnchor{initTextAnchor},
        color{initColor}
    {
        updateText();
    }

    TextRenderer::TextRenderer(const gui::Font& initFont,
                               float initFontSize,
                               const std::string& initText,
                               math::Color initColor,
                               const math::Vector<float, 2>& initTextAnchor):
        shader{engine->getCache().getShader(shaderTexture)},
        blendState{engine->getCache().getBlendState(blendAlpha)},
        indexBuffer{engine->getGraphics(),
                    graphics::BufferType::index,
                    graphics::Flags::dynamic},
        vertexBuffer{engine->getGraphics(),
                     graphics::BufferType::vertex,
                     graphics::Flags::dynamic},
        whitePixelTexture{engine->getCache().getTexture(textureWhitePixel)},
        font{&initFont},
        text{initText},
        fontSize{initFontSize},
        textAnchor{initTextAnchor},
        color{initColor}
    {
        updateText();
    }

    void TextRenderer::setFont(const std::string& fontFile)
    {
        font = engine->getCache().getFont(fontFile);

        updateText();
    }

    void TextRenderer::setTextAnchor(const math::Vector<float, 2>& newTextAnchor)
    {
        textAnchor = newTextAnchor;

        updateText();
    }

    void TextRenderer::setFontSize(float newFontSize)
    {
        fontSize = newFontSize;

        updateText();
    }

    void TextRenderer::draw(const math::Matrix<float, 4>& transformMatrix,
                            float opacity,
                            const math::Matrix<float, 4>& renderViewProjection,
                            bool wireframe)
    {
        Component::draw(transformMatrix,
                        opacity,
                        renderViewProjection,
                        wireframe);

        if (needsMeshUpdate)
        {
            indexBuffer.setData(indices.data(), static_cast<std::uint32_t>(getVectorSize(indices)));
            vertexBuffer.setData(vertices.data(), static_cast<std::uint32_t>(getVectorSize(vertices)));

            needsMeshUpdate = false;
        }

        const auto modelViewProj = renderViewProjection * transformMatrix;
        const auto colorVector = {
            color.normR(),
            color.normG(),
            color.normB(),
            color.normA() * opacity
        };

        std::vector<std::vector<float>> fragmentShaderConstants(1);
        fragmentShaderConstants[0] = {std::begin(colorVector), std::end(colorVector)};

        std::vector<std::vector<float>> vertexShaderConstants(1);
        vertexShaderConstants[0] = {std::begin(modelViewProj.m.v), std::end(modelViewProj.m.v)};

        engine->getGraphics().setPipelineState(blendState->getResource(),
                                               shader->getResource(),
                                               graphics::CullMode::none,
                                               wireframe ? graphics::FillMode::wireframe : graphics::FillMode::solid);
        engine->getGraphics().setShaderConstants(fragmentShaderConstants,
                                                 vertexShaderConstants);
        engine->getGraphics().setTextures({wireframe ? whitePixelTexture->getResource() : texture ? texture->getResource() : 0U});
        engine->getGraphics().draw(indexBuffer.getResource(),
                                   static_cast<std::uint32_t>(indices.size()),
                                   sizeof(std::uint16_t),
                                   vertexBuffer.getResource(),
                                   graphics::DrawMode::triangleList,
                                   0);
    }

    void TextRenderer::setText(const std::string& newText)
    {
        text = newText;

        updateText();
    }

    void TextRenderer::updateText()
    {
        reset(boundingBox);

        if (font)
        {
            std::tie(indices, vertices, texture) = font->getRenderData(text, math::whiteColor, fontSize, textAnchor);
            needsMeshUpdate = true;

            for (const auto& vertex : vertices)
                insertPoint(boundingBox, vertex.position);
        }
        else
        {
            indices.clear();
            vertices.clear();
            texture.reset();
        }
    }
}
<|endoftext|>