text: /******************************************************************************
   * Copyright 2018 The Apollo Authors. All Rights Reserved.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   * http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   *****************************************************************************/
  
  #include "modules/planning/common/path/discretized_path.h"
  
  #include "cyber/common/log.h"
  #include "gtest/gtest.h"
  #include "modules/common/util/point_factory.h"
  
  namespace apollo {
  namespace planning {
  
  using apollo::common::PathPoint;
  using apollo::common::util::PointFactory;
  
  TEST(DiscretizedPathTest, basic_test) {
    const double s1 = 0.0;
    const double s2 = s1 + std::sqrt(1.0 + 1.0);
    const double s3 = s2 + std::sqrt(1.0 + 1.0);
    const double s4 = s3 + std::sqrt(1.0 + 1.0);
    PathPoint p1 = PointFactory::ToPathPoint(0.0, 0.0, 0.0, s1);
    PathPoint p2 = PointFactory::ToPathPoint(1.0, 1.0, 0.0, s2);
    PathPoint p3 = PointFactory::ToPathPoint(2.0, 2.0, 0.0, s3);
    PathPoint p4 = PointFactory::ToPathPoint(3.0, 3.0, 0.0, s4);
  
    std::vector<PathPoint> path_points{p1, p2, p3, p4};
  
    DiscretizedPath discretized_path(path_points);
    EXPECT_EQ(discretized_path.size(), 4);
  
    EXPECT_DOUBLE_EQ(discretized_path.Length(), std::sqrt(1.0 + 1.0) * 3.0);
  
    auto eval_p1 = discretized_path.Evaluate(0.0);
    EXPECT_DOUBLE_EQ(eval_p1.s(), 0.0);
    EXPECT_DOUBLE_EQ(eval_p1.x(), 0.0);
    EXPECT_DOUBLE_EQ(eval_p1.y(), 0.0);
  
    auto eval_p2 = discretized_path.Evaluate(0.3 * std::sqrt(2.0));
    EXPECT_DOUBLE_EQ(eval_p2.s(), 0.3 * std::sqrt(2.0));
    EXPECT_DOUBLE_EQ(eval_p2.x(), 0.3);
    EXPECT_DOUBLE_EQ(eval_p2.y(), 0.3);
  
    auto eval_p3 = discretized_path.Evaluate(1.8);
    EXPECT_DOUBLE_EQ(eval_p3.s(), 1.8);
    EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
    EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));
  
    auto eval_p4 = discretized_path.Evaluate(2.5);
    EXPECT_DOUBLE_EQ(eval_p4.s(), 2.5);
    EXPECT_DOUBLE_EQ(eval_p4.x(), (2.0 + 0.5) / std::sqrt(2));
    EXPECT_DOUBLE_EQ(eval_p4.y(), (2.0 + 0.5) / std::sqrt(2));
  
    discretized_path.clear();
    EXPECT_EQ(discretized_path.size(), 0);
  }
  
  TEST(DiscretizedPathTest, reverse_case) {
    const double s1 = 0.0;
    const double s2 = s1 - std::sqrt(1.0 + 1.0);
    const double s3 = s2 - std::sqrt(1.0 + 1.0);
    const double s4 = s3 - std::sqrt(1.0 + 1.0);
    PathPoint p1 = PointFactory::ToPathPoint(0.0, 0.0, 0.0, s1);
    PathPoint p2 = PointFactory::ToPathPoint(1.0, 1.0, 0.0, s2);
    PathPoint p3 = PointFactory::ToPathPoint(2.0, 2.0, 0.0, s3);
    PathPoint p4 = PointFactory::ToPathPoint(3.0, 3.0, 0.0, s4);
  
    std::vector<PathPoint> path_points{p1, p2, p3, p4};
  
    DiscretizedPath discretized_path(path_points);
    EXPECT_EQ(discretized_path.size(), 4);
  
    EXPECT_DOUBLE_EQ(discretized_path.Length(), -std::sqrt(1.0 + 1.0) * 3.0);
  
    auto eval_p1 = discretized_path.EvaluateReverse(0.0);
    EXPECT_DOUBLE_EQ(eval_p1.s(), 0.0);
    EXPECT_DOUBLE_EQ(eval_p1.x(), 0.0);
    EXPECT_DOUBLE_EQ(eval_p1.y(), 0.0);
  
    auto eval_p2 = discretized_path.EvaluateReverse(-0.3 * std::sqrt(2.0));
    EXPECT_DOUBLE_EQ(eval_p2.s(), -0.3 * std::sqrt(2.0));
    EXPECT_DOUBLE_EQ(eval_p2.x(), 0.3);
    EXPECT_DOUBLE_EQ(eval_p2.y(), 0.3);
  
    auto eval_p3 = discretized_path.EvaluateReverse(-1.8);
    EXPECT_DOUBLE_EQ(eval_p3.s(), -1.8);
    EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
    EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));
  
    auto eval_p4 = discretized_path.EvaluateReverse(-2.5);
    EXPECT_DOUBLE_EQ(eval_p4.s(), -2.5);
    EXPECT_DOUBLE_EQ(eval_p4.x(), (2.0 + 0.5) / std::sqrt(2));
    EXPECT_DOUBLE_EQ(eval_p4.y(), (2.0 + 0.5) / std::sqrt(2));
  
    discretized_path.clear();
    EXPECT_EQ(discretized_path.size(), 0);
  }
  
  }  // namespace planning
  }  // namespace apollo
  