/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2007 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Tobias Leicht, 2007
 */

// deal.II包括的文件已经在前面的例子中介绍过了，因此不再做进一步的评论。

#include <deal.II/base/function.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/lac/precondition_block.h>
#include <deal.II/lac/solver_richardson.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/mapping_q1.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/derivative_approximation.h>

// 而这又是C++。

#include <array>
#include <iostream>
#include <fstream>

// 最后一步和以前所有的程序一样。

namespace Step30
{
  using namespace dealii;
// @sect3{Equation data}

// 描述方程数据的类和单个术语的实际装配几乎完全照搬自  step-12  。我们将对差异进行评论。

  template <int dim>
  class RHS : public Function<dim>
  {
  public:
    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int /*component*/ = 0) const override
    {
      (void)points;
      Assert(values.size() == points.size(),
             ExcDimensionMismatch(values.size(), points.size()));

      std::fill(values.begin(), values.end(), 0.);
    }
  };

  template <int dim>
  class BoundaryValues : public Function<dim>
  {
  public:
    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int /*component*/ = 0) const override
    {
      Assert(values.size() == points.size(),
             ExcDimensionMismatch(values.size(), points.size()));

      for (unsigned int i = 0; i < values.size(); ++i)
        {
          if (points[i](0) < 0.5)
            values[i] = 1.;
          else
            values[i] = 0.;
        }
    }
  };

  template <int dim>
  class Beta
  {
  public:

//流场选择为逆时针方向的四分之一圆，原点为域的右半部分的中点，数值为正 $x$ ，而在域的左边部分，流速只是向左走，与从右边进来的流速一致。在圆形部分，流速的大小与离原点的距离成正比。这与 step-12 不同，在该定义中，到处都是1。新定义导致 $\beta$ 沿单元的每个给定面的线性变化。另一方面， $u(x,y)$ 的解决方案与之前完全相同。

    void value_list(const std::vector<Point<dim>> &points,
                    std::vector<Point<dim>> &      values) const
    {
      Assert(values.size() == points.size(),
             ExcDimensionMismatch(values.size(), points.size()));

      for (unsigned int i = 0; i < points.size(); ++i)
        {
          if (points[i](0) > 0)
            {
              values[i](0) = -points[i](1);
              values[i](1) = points[i](0);
            }
          else
            {
              values[i]    = Point<dim>();
              values[i](0) = -points[i](1);
            }
        }
    }
  };

//  @sect3{Class: DGTransportEquation}

// 这个类的声明完全不受我们目前的变化影响。

  template <int dim>
  class DGTransportEquation
  {
  public:
    DGTransportEquation();

    void assemble_cell_term(const FEValues<dim> &fe_v,
                            FullMatrix<double> & ui_vi_matrix,
                            Vector<double> &     cell_vector) const;

    void assemble_boundary_term(const FEFaceValues<dim> &fe_v,
                                FullMatrix<double> &     ui_vi_matrix,
                                Vector<double> &         cell_vector) const;

    void assemble_face_term(const FEFaceValuesBase<dim> &fe_v,
                            const FEFaceValuesBase<dim> &fe_v_neighbor,
                            FullMatrix<double> &         ui_vi_matrix,
                            FullMatrix<double> &         ue_vi_matrix,
                            FullMatrix<double> &         ui_ve_matrix,
                            FullMatrix<double> &         ue_ve_matrix) const;

  private:
    const Beta<dim>           beta_function;
    const RHS<dim>            rhs_function;
    const BoundaryValues<dim> boundary_function;
  };

// 同样地，该类的构造函数以及组装对应于单元格内部和边界面的术语的函数与之前没有变化。装配单元间面术语的函数也没有改变，因为它所做的只是对两个FEFaceValuesBase类型的对象进行操作（它是FEFaceValues和FESubfaceValues的基类）。这些对象从何而来，即它们是如何被初始化的，对这个函数来说并不重要：它只是假设这两个对象所代表的面或子面上的正交点与物理空间中的相同点相对应。

  template <int dim>
  DGTransportEquation<dim>::DGTransportEquation()
    : beta_function()
    , rhs_function()
    , boundary_function()
  {}

  template <int dim>
  void DGTransportEquation<dim>::assemble_cell_term(
    const FEValues<dim> &fe_v,
    FullMatrix<double> & ui_vi_matrix,
    Vector<double> &     cell_vector) const
  {
    const std::vector<double> &JxW = fe_v.get_JxW_values();

    std::vector<Point<dim>> beta(fe_v.n_quadrature_points);
    std::vector<double>     rhs(fe_v.n_quadrature_points);

    beta_function.value_list(fe_v.get_quadrature_points(), beta);
    rhs_function.value_list(fe_v.get_quadrature_points(), rhs);

    for (unsigned int point = 0; point < fe_v.n_quadrature_points; ++point)
      for (unsigned int i = 0; i < fe_v.dofs_per_cell; ++i)
        {
          for (unsigned int j = 0; j < fe_v.dofs_per_cell; ++j)
            ui_vi_matrix(i, j) -= beta[point] * fe_v.shape_grad(i, point) *
                                  fe_v.shape_value(j, point) * JxW[point];

          cell_vector(i) +=
            rhs[point] * fe_v.shape_value(i, point) * JxW[point];
        }
  }

  template <int dim>
  void DGTransportEquation<dim>::assemble_boundary_term(
    const FEFaceValues<dim> &fe_v,
    FullMatrix<double> &     ui_vi_matrix,
    Vector<double> &         cell_vector) const
  {
    const std::vector<double> &        JxW     = fe_v.get_JxW_values();
    const std::vector<Tensor<1, dim>> &normals = fe_v.get_normal_vectors();

    std::vector<Point<dim>> beta(fe_v.n_quadrature_points);
    std::vector<double>     g(fe_v.n_quadrature_points);

    beta_function.value_list(fe_v.get_quadrature_points(), beta);
    boundary_function.value_list(fe_v.get_quadrature_points(), g);

    for (unsigned int point = 0; point < fe_v.n_quadrature_points; ++point)
      {
        const double beta_n = beta[point] * normals[point];
        if (beta_n > 0)
          for (unsigned int i = 0; i < fe_v.dofs_per_cell; ++i)
            for (unsigned int j = 0; j < fe_v.dofs_per_cell; ++j)
              ui_vi_matrix(i, j) += beta_n * fe_v.shape_value(j, point) *
                                    fe_v.shape_value(i, point) * JxW[point];
        else
          for (unsigned int i = 0; i < fe_v.dofs_per_cell; ++i)
            cell_vector(i) -=
              beta_n * g[point] * fe_v.shape_value(i, point) * JxW[point];
      }
  }

  template <int dim>
  void DGTransportEquation<dim>::assemble_face_term(
    const FEFaceValuesBase<dim> &fe_v,
    const FEFaceValuesBase<dim> &fe_v_neighbor,
    FullMatrix<double> &         ui_vi_matrix,
    FullMatrix<double> &         ue_vi_matrix,
    FullMatrix<double> &         ui_ve_matrix,
    FullMatrix<double> &         ue_ve_matrix) const
  {
    const std::vector<double> &        JxW     = fe_v.get_JxW_values();
    const std::vector<Tensor<1, dim>> &normals = fe_v.get_normal_vectors();

    std::vector<Point<dim>> beta(fe_v.n_quadrature_points);

    beta_function.value_list(fe_v.get_quadrature_points(), beta);

    for (unsigned int point = 0; point < fe_v.n_quadrature_points; ++point)
      {
        const double beta_n = beta[point] * normals[point];
        if (beta_n > 0)
          {
            for (unsigned int i = 0; i < fe_v.dofs_per_cell; ++i)
              for (unsigned int j = 0; j < fe_v.dofs_per_cell; ++j)
                ui_vi_matrix(i, j) += beta_n * fe_v.shape_value(j, point) *
                                      fe_v.shape_value(i, point) * JxW[point];

            for (unsigned int k = 0; k < fe_v_neighbor.dofs_per_cell; ++k)
              for (unsigned int j = 0; j < fe_v.dofs_per_cell; ++j)
                ui_ve_matrix(k, j) -= beta_n * fe_v.shape_value(j, point) *
                                      fe_v_neighbor.shape_value(k, point) *
                                      JxW[point];
          }
        else
          {
            for (unsigned int i = 0; i < fe_v.dofs_per_cell; ++i)
              for (unsigned int l = 0; l < fe_v_neighbor.dofs_per_cell; ++l)
                ue_vi_matrix(i, l) += beta_n *
                                      fe_v_neighbor.shape_value(l, point) *
                                      fe_v.shape_value(i, point) * JxW[point];

            for (unsigned int k = 0; k < fe_v_neighbor.dofs_per_cell; ++k)
              for (unsigned int l = 0; l < fe_v_neighbor.dofs_per_cell; ++l)
                ue_ve_matrix(k, l) -=
                  beta_n * fe_v_neighbor.shape_value(l, point) *
                  fe_v_neighbor.shape_value(k, point) * JxW[point];
          }
      }
  }
// @sect3{Class: DGMethod}

// 这个声明很像  step-12  的声明。然而，我们引入了一个新的例程（set_anisotropic_flags）并修改了另一个例程（refine_grid）。

  template <int dim>
  class DGMethod
  {
  public:
    DGMethod(const bool anisotropic);

    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve(Vector<double> &solution);
    void refine_grid();
    void set_anisotropic_flags();
    void output_results(const unsigned int cycle) const;

    Triangulation<dim>   triangulation;
    const MappingQ1<dim> mapping;

// 我们再次希望使用程度为1的DG元素（但这只在构造函数中指定）。如果你想使用不同程度的DG方法，请在构造函数中用新的程度替换1。

    const unsigned int degree;
    FE_DGQ<dim>        fe;
    DoFHandler<dim>    dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

// 这是新的，在介绍中解释的各向异性跳跃指标的评估中使用的阈值。它的值在构造函数中被设置为3.0，但它可以很容易地被改变为一个大于1的不同值。

    const double anisotropic_threshold_ratio;

// 这是一个指示是否使用各向异性细化的bool标志。它由构造函数设置，构造函数需要一个同名的参数。

    const bool anisotropic;

    const QGauss<dim>     quadrature;
    const QGauss<dim - 1> face_quadrature;

    Vector<double> solution2;
    Vector<double> right_hand_side;

    const DGTransportEquation<dim> dg;
  };

  template <int dim>
  DGMethod<dim>::DGMethod(const bool anisotropic)
    : mapping()
    ,

// 对于不同程度的DG方法，在这里进行修改。

    degree(1)
    , fe(degree)
    , dof_handler(triangulation)
    , anisotropic_threshold_ratio(3.)
    , anisotropic(anisotropic)
    ,

// 由于β是一个线性函数，我们可以选择正交的度数，对于这个度数，所得的积分是正确的。因此，我们选择使用  degree+1  高斯点，这使我们能够准确地积分度数为  2*degree+1  的多项式，足以满足我们在本程序中要进行的所有积分。

    quadrature(degree + 1)
    , face_quadrature(degree + 1)
    , dg()
  {}

  template <int dim>
  void DGMethod<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);
    sparsity_pattern.reinit(dof_handler.n_dofs(),
                            dof_handler.n_dofs(),
                            (GeometryInfo<dim>::faces_per_cell *
                               GeometryInfo<dim>::max_children_per_face +
                             1) *
                              fe.n_dofs_per_cell());

    DoFTools::make_flux_sparsity_pattern(dof_handler, sparsity_pattern);

    sparsity_pattern.compress();

    system_matrix.reinit(sparsity_pattern);

    solution2.reinit(dof_handler.n_dofs());
    right_hand_side.reinit(dof_handler.n_dofs());
  }
// @sect4{Function: assemble_system}

// 我们继续使用 assemble_system 函数来实现DG离散化。这个函数与 step-12 中的 assemble_system 函数的作用相同（但没有MeshWorker）。 一个单元的邻居关系所考虑的四种情况与各向同性的情况相同，即a)单元在边界上，b)有更细的邻居单元，c)邻居既不粗也不细，d)邻居更粗。 然而，我们决定哪种情况的方式是按照介绍中描述的方式进行修改的。

  template <int dim>
  void DGMethod<dim>::assemble_system()
  {
    const unsigned int dofs_per_cell = dof_handler.get_fe().n_dofs_per_cell();
    std::vector<types::global_dof_index> dofs(dofs_per_cell);
    std::vector<types::global_dof_index> dofs_neighbor(dofs_per_cell);

    const UpdateFlags update_flags = update_values | update_gradients |
                                     update_quadrature_points |
                                     update_JxW_values;

    const UpdateFlags face_update_flags =
      update_values | update_quadrature_points | update_JxW_values |
      update_normal_vectors;

    const UpdateFlags neighbor_face_update_flags = update_values;

    FEValues<dim>        fe_v(mapping, fe, quadrature, update_flags);
    FEFaceValues<dim>    fe_v_face(mapping,
                                fe,
                                face_quadrature,
                                face_update_flags);
    FESubfaceValues<dim> fe_v_subface(mapping,
                                      fe,
                                      face_quadrature,
                                      face_update_flags);
    FEFaceValues<dim>    fe_v_face_neighbor(mapping,
                                         fe,
                                         face_quadrature,
                                         neighbor_face_update_flags);

    FullMatrix<double> ui_vi_matrix(dofs_per_cell, dofs_per_cell);
    FullMatrix<double> ue_vi_matrix(dofs_per_cell, dofs_per_cell);

    FullMatrix<double> ui_ve_matrix(dofs_per_cell, dofs_per_cell);
    FullMatrix<double> ue_ve_matrix(dofs_per_cell, dofs_per_cell);

    Vector<double> cell_vector(dofs_per_cell);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        ui_vi_matrix = 0;
        cell_vector  = 0;

        fe_v.reinit(cell);

        dg.assemble_cell_term(fe_v, ui_vi_matrix, cell_vector);

        cell->get_dof_indices(dofs);

        for (const auto face_no : cell->face_indices())
          {
            const auto face = cell->face(face_no);

// 情况(a)。该面在边界上。

            if (face->at_boundary())
              {
                fe_v_face.reinit(cell, face_no);

                dg.assemble_boundary_term(fe_v_face, ui_vi_matrix, cell_vector);
              }
            else
              {
                Assert(cell->neighbor(face_no).state() == IteratorState::valid,
                       ExcInternalError());
                const auto neighbor = cell->neighbor(face_no);

// 情况(b)。这是一个内部面，邻居是精炼的（我们可以通过询问当前单元格的面是否有孩子来测试）。在这种情况下，我们需要对 "子面 "进行整合，即当前单元格的面的子女。            (有一个稍微令人困惑的角落案例。如果我们是在1d中--诚然，当前的程序和它对各向异性细化的演示并不特别相关--那么单元间的面总是相同的：它们只是顶点。换句话说，在1d中，我们不希望对不同层次的单元之间的面进行不同的处理。我们在这里检查的条件`face->has_children()`确保了这一点：在1d中，这个函数总是返回`false`，因此在1d中我们不会进入这个`if`分支。但我们将不得不在下面的情况（c）中回到这个角落。

                if (face->has_children())
                  {

// 我们需要知道，哪个邻居的面朝向我们单元格的方向。使用  @p  neighbor_face_no 函数，我们可以得到粗邻和非粗邻的这些信息。

                    const unsigned int neighbor2 =
                      cell->neighbor_face_no(face_no);

// 现在我们对所有的子脸进行循环，也就是当前脸的子脸和可能的孙子脸。

                    for (unsigned int subface_no = 0;
                         subface_no < face->n_active_descendants();
                         ++subface_no)
                      {

// 为了得到当前子面后面的单元，我们可以使用 @p neighbor_child_on_subface 函数。它照顾到了所有各向异性细化和非标准面的复杂情况。

                        const auto neighbor_child =
                          cell->neighbor_child_on_subface(face_no, subface_no);
                        Assert(!neighbor_child->has_children(),
                               ExcInternalError());

// 这个案例的其余部分没有变化。

                        ue_vi_matrix = 0;
                        ui_ve_matrix = 0;
                        ue_ve_matrix = 0;

                        fe_v_subface.reinit(cell, face_no, subface_no);
                        fe_v_face_neighbor.reinit(neighbor_child, neighbor2);

                        dg.assemble_face_term(fe_v_subface,
                                              fe_v_face_neighbor,
                                              ui_vi_matrix,
                                              ue_vi_matrix,
                                              ui_ve_matrix,
                                              ue_ve_matrix);

                        neighbor_child->get_dof_indices(dofs_neighbor);

                        for (unsigned int i = 0; i < dofs_per_cell; ++i)
                          for (unsigned int j = 0; j < dofs_per_cell; ++j)
                            {
                              system_matrix.add(dofs[i],
                                                dofs_neighbor[j],
                                                ue_vi_matrix(i, j));
                              system_matrix.add(dofs_neighbor[i],
                                                dofs[j],
                                                ui_ve_matrix(i, j));
                              system_matrix.add(dofs_neighbor[i],
                                                dofs_neighbor[j],
                                                ue_ve_matrix(i, j));
                            }
                      }
                  }
                else
                  {

//情况(c)。如果这是一个内部面，并且邻居没有进一步细化，我们就会得到这里（或者，如上所述，我们是在1d中，在这种情况下，我们对每个内部面都会得到这里）。然后我们需要决定是否要对当前面进行整合。如果邻居实际上更粗，那么我们就忽略这个面，而是在访问邻居单元并查看当前面的时候处理它（除了在1d中，如上所述，这不会发生）。

                    if (dim > 1 && cell->neighbor_is_coarser(face_no))
                      continue;

// 另一方面，如果邻居是更精细的，那么我们已经处理了上面(b)情况下的脸（1d除外）。所以对于2d和3d，我们只需要决定是要处理来自当前一侧的同一层次的单元格之间的面，还是来自邻接一侧的面。 我们通过引入一个平局来做到这一点。          我们只取索引较小的单元格（在当前细化级别内）。在1d中，我们取较粗的单元，或者如果它们在同一层次，则取该层次中指数较小的单元。这就导致了一个复杂的条件，希望在上面的描述中可以理解。

                    if (((dim > 1) && (cell->index() < neighbor->index())) ||
                        ((dim == 1) && ((cell->level() < neighbor->level()) ||
                                        ((cell->level() == neighbor->level()) &&
                                         (cell->index() < neighbor->index())))))
                      {

// 这里我们知道，邻居不是更粗的，所以我们可以使用通常的  @p neighbor_of_neighbor  函数。然而，我们也可以使用更通用的 @p neighbor_face_no 函数。

                        const unsigned int neighbor2 =
                          cell->neighbor_of_neighbor(face_no);

                        ue_vi_matrix = 0;
                        ui_ve_matrix = 0;
                        ue_ve_matrix = 0;

                        fe_v_face.reinit(cell, face_no);
                        fe_v_face_neighbor.reinit(neighbor, neighbor2);

                        dg.assemble_face_term(fe_v_face,
                                              fe_v_face_neighbor,
                                              ui_vi_matrix,
                                              ue_vi_matrix,
                                              ui_ve_matrix,
                                              ue_ve_matrix);

                        neighbor->get_dof_indices(dofs_neighbor);

                        for (unsigned int i = 0; i < dofs_per_cell; ++i)
                          for (unsigned int j = 0; j < dofs_per_cell; ++j)
                            {
                              system_matrix.add(dofs[i],
                                                dofs_neighbor[j],
                                                ue_vi_matrix(i, j));
                              system_matrix.add(dofs_neighbor[i],
                                                dofs[j],
                                                ui_ve_matrix(i, j));
                              system_matrix.add(dofs_neighbor[i],
                                                dofs_neighbor[j],
                                                ue_ve_matrix(i, j));
                            }
                      }

// 我们不需要考虑情况(d)，因为这些面在情况(b)中被 "从另一侧 "处理。

                  }
              }
          }

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            system_matrix.add(dofs[i], dofs[j], ui_vi_matrix(i, j));

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          right_hand_side(dofs[i]) += cell_vector(i);
      }
  }
// @sect3{Solver}

// 对于这个简单的问题，我们再次使用简单的Richardson迭代法。该求解器完全不受我们各向异性变化的影响。

  template <int dim>
  void DGMethod<dim>::solve(Vector<double> &solution)
  {
    SolverControl                    solver_control(1000, 1e-12, false, false);
    SolverRichardson<Vector<double>> solver(solver_control);

    PreconditionBlockSSOR<SparseMatrix<double>> preconditioner;

    preconditioner.initialize(system_matrix, fe.n_dofs_per_cell());

    solver.solve(system_matrix, solution, right_hand_side, preconditioner);
  }
// @sect3{Refinement}

// 我们根据 step-12 中使用的相同的简单细化标准来细化网格，即对解的梯度的近似。

  template <int dim>
  void DGMethod<dim>::refine_grid()
  {
    Vector<float> gradient_indicator(triangulation.n_active_cells());

// 我们对梯度进行近似计算。

    DerivativeApproximation::approximate_gradient(mapping,
                                                  dof_handler,
                                                  solution2,
                                                  gradient_indicator);

//并对其进行缩放，以获得一个误差指标。

    for (const auto &cell : triangulation.active_cell_iterators())
      gradient_indicator[cell->active_cell_index()] *=
        std::pow(cell->diameter(), 1 + 1.0 * dim / 2);

// 然后我们用这个指标来标记误差指标最高的30%的单元格来进行精炼。

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    gradient_indicator,
                                                    0.3,
                                                    0.1);

// 现在，细化标志被设置为那些具有大误差指标的单元。如果不做任何改变，这些单元将被等向细化。如果给这个函数的 @p anisotropic 标志被设置，我们现在调用set_anisotropic_flags()函数，该函数使用跳转指标将一些细化标志重置为各向异性细化。

    if (anisotropic)
      set_anisotropic_flags();

// 现在执行考虑各向异性以及各向同性的细化标志的细化。

    triangulation.execute_coarsening_and_refinement();
  }

// 一旦错误指标被评估，误差最大的单元被标记为细化，我们要再次循环这些被标记的单元，以决定它们是否需要各向同性的细化或各向异性的细化更为合适。这就是在介绍中解释的各向异性跳跃指标。

  template <int dim>
  void DGMethod<dim>::set_anisotropic_flags()
  {

// 我们想在被标记的单元格的面上评估跳跃，所以我们需要一些对象来评估面上的解决方案的值。

    UpdateFlags face_update_flags =
      UpdateFlags(update_values | update_JxW_values);

    FEFaceValues<dim>    fe_v_face(mapping,
                                fe,
                                face_quadrature,
                                face_update_flags);
    FESubfaceValues<dim> fe_v_subface(mapping,
                                      fe,
                                      face_quadrature,
                                      face_update_flags);
    FEFaceValues<dim>    fe_v_face_neighbor(mapping,
                                         fe,
                                         face_quadrature,
                                         update_values);

// 现在我们需要对所有活动单元进行循环。

    for (const auto &cell : dof_handler.active_cell_iterators())

// 我们只需要考虑那些被标记为细化的单元。

      if (cell->refine_flag_set())
        {
          Point<dim> jump;
          Point<dim> area;

          for (const auto face_no : cell->face_indices())
            {
              const auto face = cell->face(face_no);

              if (!face->at_boundary())
                {
                  Assert(cell->neighbor(face_no).state() ==
                           IteratorState::valid,
                         ExcInternalError());
                  const auto neighbor = cell->neighbor(face_no);

                  std::vector<double> u(fe_v_face.n_quadrature_points);
                  std::vector<double> u_neighbor(fe_v_face.n_quadrature_points);

// 在汇编例程中看到的四种不同的邻居关系的情况在这里以同样的方式重复。

                  if (face->has_children())
                    {

// 邻居被完善。 首先，我们存储信息，即邻居的哪个面指向我们当前单元的方向。这个属性将被继承给子代。

                      unsigned int neighbor2 = cell->neighbor_face_no(face_no);

// 现在我们对所有的子面进行循环。

                      for (unsigned int subface_no = 0;
                           subface_no < face->n_active_descendants();
                           ++subface_no)
                        {

//得到一个迭代器，指向当前子面后面的单元格...

                          const auto neighbor_child =
                            cell->neighbor_child_on_subface(face_no,
                                                            subface_no);
                          Assert(!neighbor_child->has_children(),
                                 ExcInternalError());

// ...并重新启动各自的FEFaceValues和FESSubFaceValues对象。

                          fe_v_subface.reinit(cell, face_no, subface_no);
                          fe_v_face_neighbor.reinit(neighbor_child, neighbor2);

// 我们获得了函数值

                          fe_v_subface.get_function_values(solution2, u);
                          fe_v_face_neighbor.get_function_values(solution2,
                                                                 u_neighbor);

//以及正交权重，乘以雅各布行列式。

                          const std::vector<double> &JxW =
                            fe_v_subface.get_JxW_values();

// 现在我们在所有的正交点上循环。

                          for (unsigned int x = 0;
                               x < fe_v_subface.n_quadrature_points;
                               ++x)
                            {

//并整合解决方案的跳跃的绝对值，即分别从当前单元和邻近单元看到的函数值的绝对值。我们知道，前两个面与单元格上的第一个坐标方向正交，后两个面与第二个坐标方向正交，以此类推，所以我们将这些值累积成具有 dim 成分的向量。

                              jump[face_no / 2] +=
                                std::abs(u[x] - u_neighbor[x]) * JxW[x];

// 我们还将缩放后的权重相加，以获得脸部的量度。

                              area[face_no / 2] += JxW[x];
                            }
                        }
                    }
                  else
                    {
                      if (!cell->neighbor_is_coarser(face_no))
                        {

// 我们的当前单元和邻居在所考虑的面有相同的细化。除此以外，我们的做法与上述情况下的一个子单元的做法基本相同。

                          unsigned int neighbor2 =
                            cell->neighbor_of_neighbor(face_no);

                          fe_v_face.reinit(cell, face_no);
                          fe_v_face_neighbor.reinit(neighbor, neighbor2);

                          fe_v_face.get_function_values(solution2, u);
                          fe_v_face_neighbor.get_function_values(solution2,
                                                                 u_neighbor);

                          const std::vector<double> &JxW =
                            fe_v_face.get_JxW_values();

                          for (unsigned int x = 0;
                               x < fe_v_face.n_quadrature_points;
                               ++x)
                            {
                              jump[face_no / 2] +=
                                std::abs(u[x] - u_neighbor[x]) * JxW[x];
                              area[face_no / 2] += JxW[x];
                            }
                        }
                      else // i.e. neighbor is coarser than cell
                        {

// 现在邻居实际上更粗了。这种情况是新的，因为它没有出现在汇编程序中。在这里，我们必须考虑它，但这并不太复杂。我们只需使用  @p  neighbor_of_coarser_neighbor 函数，它再次自行处理各向异性的细化和非标准面的方向。

                          std::pair<unsigned int, unsigned int>
                            neighbor_face_subface =
                              cell->neighbor_of_coarser_neighbor(face_no);
                          Assert(neighbor_face_subface.first < cell->n_faces(),
                                 ExcInternalError());
                          Assert(neighbor_face_subface.second <
                                   neighbor->face(neighbor_face_subface.first)
                                     ->n_active_descendants(),
                                 ExcInternalError());
                          Assert(neighbor->neighbor_child_on_subface(
                                   neighbor_face_subface.first,
                                   neighbor_face_subface.second) == cell,
                                 ExcInternalError());

                          fe_v_face.reinit(cell, face_no);
                          fe_v_subface.reinit(neighbor,
                                              neighbor_face_subface.first,
                                              neighbor_face_subface.second);

                          fe_v_face.get_function_values(solution2, u);
                          fe_v_subface.get_function_values(solution2,
                                                           u_neighbor);

                          const std::vector<double> &JxW =
                            fe_v_face.get_JxW_values();

                          for (unsigned int x = 0;
                               x < fe_v_face.n_quadrature_points;
                               ++x)
                            {
                              jump[face_no / 2] +=
                                std::abs(u[x] - u_neighbor[x]) * JxW[x];
                              area[face_no / 2] += JxW[x];
                            }
                        }
                    }
                }
            }

// 现在我们分析一下平均跳动的大小，我们用跳动除以各面的度量得到。

          std::array<double, dim> average_jumps;
          double                  sum_of_average_jumps = 0.;
          for (unsigned int i = 0; i < dim; ++i)
            {
              average_jumps[i] = jump(i) / area(i);
              sum_of_average_jumps += average_jumps[i];
            }

// 现在我们在单元格的 dim 坐标方向上进行循环，并比较与该方向正交的面的平均跳跃和与其余方向正交的面的平均跳跃。如果前者比后者大一个给定的系数，我们只沿帽轴进行细化。否则，我们不改变细化标志，导致各向同性的细化。

          for (unsigned int i = 0; i < dim; ++i)
            if (average_jumps[i] > anisotropic_threshold_ratio *
                                     (sum_of_average_jumps - average_jumps[i]))
              cell->set_refine_flag(RefinementCase<dim>::cut_axis(i));
        }
  }
// @sect3{The Rest}

// 程序的其余部分非常遵循之前教程程序的方案。我们以VTU格式输出网格（就像我们在 step-1 中所做的那样，例如），并以VTU格式输出可视化，我们几乎总是这样做。

  template <int dim>
  void DGMethod<dim>::output_results(const unsigned int cycle) const
  {
    std::string refine_type;
    if (anisotropic)
      refine_type = ".aniso";
    else
      refine_type = ".iso";

    {
      const std::string filename =
        "grid-" + std::to_string(cycle) + refine_type + ".svg";
      std::cout << "   Writing grid to <" << filename << ">..." << std::endl;
      std::ofstream svg_output(filename);

      GridOut grid_out;
      grid_out.write_svg(triangulation, svg_output);
    }

    {
      const std::string filename =
        "sol-" + std::to_string(cycle) + refine_type + ".vtu";
      std::cout << "   Writing solution to <" << filename << ">..."
                << std::endl;
      std::ofstream gnuplot_output(filename);

      DataOut<dim> data_out;
      data_out.attach_dof_handler(dof_handler);
      data_out.add_data_vector(solution2, "u");

      data_out.build_patches(degree);

      data_out.write_vtu(gnuplot_output);
    }
  }

  template <int dim>
  void DGMethod<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 6; ++cycle)
      {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          {

// 创建矩形域。

            Point<dim> p1, p2;
            p1(0) = 0;
            p1(0) = -1;
            for (unsigned int i = 0; i < dim; ++i)
              p2(i) = 1.;

// 调整不同方向的单元数，以获得原始网格的完全各向同性的单元。

            std::vector<unsigned int> repetitions(dim, 1);
            repetitions[0] = 2;
            GridGenerator::subdivided_hyper_rectangle(triangulation,
                                                      repetitions,
                                                      p1,
                                                      p2);

            triangulation.refine_global(5 - dim);
          }
        else
          refine_grid();

        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;

        setup_system();

        std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                  << std::endl;

        Timer assemble_timer;
        assemble_system();
        std::cout << "   Time of assemble_system: " << assemble_timer.cpu_time()
                  << std::endl;
        solve(solution2);

        output_results(cycle);

        std::cout << std::endl;
      }
  }
} // namespace Step30

int main()
{
  try
    {
      using namespace Step30;

// 如果你想以3D方式运行程序，只需将下面一行改为 const unsigned int dim = 3;  。

      const unsigned int dim = 2;

      {

// 首先，我们用各向同性的细化方法进行一次运行。

        std::cout << "Performing a " << dim
                  << "D run with isotropic refinement..." << std::endl
                  << "------------------------------------------------"
                  << std::endl;
        DGMethod<dim> dgmethod_iso(false);
        dgmethod_iso.run();
      }

      {

// 现在我们进行第二次运行，这次是各向异性的细化。

        std::cout << std::endl
                  << "Performing a " << dim
                  << "D run with anisotropic refinement..." << std::endl
                  << "--------------------------------------------------"
                  << std::endl;
        DGMethod<dim> dgmethod_aniso(true);
        dgmethod_aniso.run();
      }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    };

  return 0;
}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#ifndef LIMBO_OPT_ADAM_HPP
#define LIMBO_OPT_ADAM_HPP

#include <algorithm>

#include <Eigen/Core>

#include <limbo/opt/optimizer.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/tools/math.hpp>

namespace limbo {
    namespace defaults {
        struct opt_adam {
            /// @ingroup opt_defaults
            /// number of max iterations
            BO_PARAM(int, iterations, 300);

            /// @ingroup opt_defaults
            /// alpha - learning rate
            BO_PARAM(double, alpha, 0.001);

            /// @ingroup opt_defaults
            /// β1
            BO_PARAM(double, b1, 0.9);

            /// @ingroup opt_defaults
            /// β2
            BO_PARAM(double, b2, 0.999);

            /// @ingroup opt_defaults
            /// norm epsilon for stopping
            BO_PARAM(double, eps_stop, 0.0);
        };
    } // namespace defaults
    namespace opt {
        /// @ingroup opt
        /// Adam optimizer
        /// Equations from: http://ruder.io/optimizing-gradient-descent/index.html#gradientdescentoptimizationalgorithms
        /// (I changed a bit the notation; η to α)
        ///
        /// Parameters:
        /// - int iterations
        /// - double alpha
        /// - double b1
        /// - double b2
        /// - double eps_stop
        template <typename Params>
        struct Adam {
            template <typename F>
            Eigen::VectorXd operator()(const F& f, const Eigen::VectorXd& init, bool bounded) const
            {
                assert(Params::opt_adam::b1() >= 0. && Params::opt_adam::b1() < 1.);
                assert(Params::opt_adam::b2() >= 0. && Params::opt_adam::b2() < 1.);
                assert(Params::opt_adam::alpha() >= 0.);

                size_t param_dim = init.size();
                double b1 = Params::opt_adam::b1();
                double b2 = Params::opt_adam::b2();
                double b1_t = b1;
                double b2_t = b2;
                double alpha = Params::opt_adam::alpha();
                double stop = Params::opt_adam::eps_stop();
                double epsilon = 1e-8;

                Eigen::VectorXd m = Eigen::VectorXd::Zero(param_dim);
                Eigen::VectorXd v = Eigen::VectorXd::Zero(param_dim);

                Eigen::VectorXd params = init;

                if (bounded) {
                    for (int j = 0; j < params.size(); j++) {
                        if (params(j) < 0)
                            params(j) = 0;
                        if (params(j) > 1)
                            params(j) = 1;
                    }
                }

                for (int i = 0; i < Params::opt_adam::iterations(); ++i) {
                    Eigen::VectorXd prev_params = params;
                    auto perf = opt::eval_grad(f, params);

                    Eigen::VectorXd grad = opt::grad(perf);
                    m = b1 * m.array() + (1. - b1) * grad.array();
                    v = b2 * v.array() + (1. - b2) * grad.array().square();

                    Eigen::VectorXd m_hat = m.array() / (1. - b1_t);
                    Eigen::VectorXd v_hat = v.array() / (1. - b2_t);

                    params.array() += alpha * m_hat.array() / (v_hat.array().sqrt() + epsilon);

                    b1_t *= b1;
                    b2_t *= b2;

                    if (bounded) {
                        for (int j = 0; j < params.size(); j++) {
                            if (params(j) < 0)
                                params(j) = 0;
                            if (params(j) > 1)
                                params(j) = 1;
                        }
                    }

                    if ((prev_params - params).norm() < stop)
                        break;
                }

                return params;
            }
        };
    } // namespace opt
} // namespace limbo

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_GAUSS_SEIDEL_INCLUDE
#define ITL_GAUSS_SEIDEL_INCLUDE

#include <boost/assert.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace itl {

/// Gauss-Seidel smoother
/** Constructor takes references to a matrix and a right-hand side vector.
    operator() is applied on a vector and changes it in place.
    Matrix must be square, stored row-major and free of zero entries in the diagonal.
    Vectors b and x must have the same number of rows as A.
**/
template <typename Matrix>
class gauss_seidel
{
    typedef typename mtl::Collection<Matrix>::value_type Scalar;
    typedef typename mtl::Collection<Matrix>::size_type  size_type;
  public:
    /// Construct with constant references to matrix and RHS vector
    gauss_seidel(const Matrix& A) : A(A), dia_inv(num_rows(A))
    {
	BOOST_STATIC_ASSERT((mtl::traits::is_row_major<Matrix>::value)); // No CCS
	assert(num_rows(A) == num_cols(A)); // Matrix must be square
	for (size_type i= 0; i < num_rows(A); ++i) {
	    Scalar a= A[i][i];
	    MTL_THROW_IF(a == 0, mtl::missing_diagonal());
	    dia_inv[i]= 1.0 / a;
	}
    }

    /// Apply Gauss-Seidel on vector \p x, i.e. \p x is changed
    template <typename Vector, typename RHSVector>
    Vector& operator()(Vector& x, const RHSVector& b) const
    {
	mtl::vampir_trace<8551> tracer;
	namespace tag= mtl::tag; using namespace mtl::traits;
	using mtl::begin; using mtl::end;

        typedef typename range_generator<tag::row, Matrix>::type       a_cur_type;
        typedef typename range_generator<tag::nz, a_cur_type>::type    a_icur_type;
	typename col<Matrix>::type                   col_a(A);
	typename const_value<Matrix>::type           value_a(A);

	typedef typename mtl::Collection<Vector>::value_type           value_type;

	a_cur_type ac= begin<tag::row>(A), aend= end<tag::row>(A);
	for (unsigned i= 0; ac != aend; ++ac, ++i) {
	    value_type tmp= b[i];
	    for (a_icur_type aic= begin<tag::nz>(ac), aiend= end<tag::nz>(ac); aic != aiend; ++aic)
		if (col_a(*aic) != i)
		    tmp-= value_a(*aic) * x[col_a(*aic)];
	    x[i]= dia_inv[i] * tmp;
	}
 	return x;
    }

   private:
    const Matrix&    A;
    mtl::dense_vector<Scalar>  dia_inv;
};

template <typename Value, typename Parameters>
class gauss_seidel<mtl::mat::compressed2D<Value, Parameters> >
{
    typedef mtl::mat::compressed2D<Value, Parameters> Matrix;
    typedef typename mtl::Collection<Matrix>::value_type Scalar;
    typedef typename mtl::Collection<Matrix>::size_type  size_type;
  public:
    /// Construct with constant references to matrix and RHS vector
    gauss_seidel(const Matrix& A)
      : A(A), dia_inv(num_rows(A)), dia_pos(num_rows(A))
    {
	BOOST_STATIC_ASSERT((mtl::traits::is_row_major<Matrix>::value)); // No CCS
	assert(num_rows(A) == num_cols(A)); // Matrix must be square
	for (size_type i= 0; i < num_rows(A); ++i) {
	    mtl::utilities::maybe<size_type> pos = A.indexer(A, i, i);
	    MTL_THROW_IF(!pos, mtl::missing_diagonal());
	    dia_inv[i]= 1.0 / A.value_from_offset(pos);
	    dia_pos[i]= pos;
	}
    }

    /// Apply Gauss-Seidel on vector \p x, i.e. \p x is changed
    template <typename Vector, typename RHSVector>
    Vector& operator()(Vector& x, const RHSVector& b) const
    {
	mtl::vampir_trace<8551> tracer;
	typedef typename mtl::Collection<Vector>::value_type           value_type;
	typedef typename mtl::Collection<Matrix>::size_type            size_type;
	const size_type nr= num_rows(A);
	size_type cj1= A.ref_major()[0];
	for (size_type i= 0; i < nr; ++i) {
	    value_type tmp= b[i];
	    size_type cj0= cj1, cjm= dia_pos[i];
	    cj1= A.ref_major()[i+1];
	    for (; cj0 < cjm; cj0++)
		tmp-= A.data[cj0] * x[A.ref_minor()[cj0]];
	    for (size_type j= cjm+1; j < cj1; j++)
		tmp-= A.data[j] * x[A.ref_minor()[j]];
	    x[i]= dia_inv[i] * tmp;
	}
 	return x;
    }

  private:
    const Matrix&    A;
    mtl::dense_vector<Scalar>     dia_inv;
    mtl::dense_vector<size_type>  dia_pos;
};

} // namespace itl

#endif // ITL_GAUSS_SEIDEL_INCLUDE


/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef GCOPTER_HPP
#define GCOPTER_HPP

#include "gcopter/minco.hpp"
#include "gcopter/flatness.hpp"
#include "gcopter/lbfgs.hpp"

#include <Eigen/Eigen>

#include <cmath>
#include <cfloat>
#include <iostream>
#include <vector>

namespace gcopter
{

    class GCOPTER_PolytopeSFC
    {
    public:
        typedef Eigen::Matrix3Xd PolyhedronV;
        typedef Eigen::MatrixX4d PolyhedronH;
        typedef std::vector<PolyhedronV> PolyhedraV;
        typedef std::vector<PolyhedronH> PolyhedraH;

    private:
        minco::MINCO_S3NU minco;
        flatness::FlatnessMap flatmap;

        double rho;
        Eigen::Matrix3d headPVA;
        Eigen::Matrix3d tailPVA;

        PolyhedraV vPolytopes;
        PolyhedraH hPolytopes;
        Eigen::Matrix3Xd shortPath;

        Eigen::VectorXi pieceIdx;
        Eigen::VectorXi vPolyIdx;
        Eigen::VectorXi hPolyIdx;

        int polyN;
        int pieceN;

        int spatialDim;
        int temporalDim;

        double smoothEps;
        int integralRes;
        Eigen::VectorXd magnitudeBd;
        Eigen::VectorXd penaltyWt;
        Eigen::VectorXd physicalPm;
        double allocSpeed;

        lbfgs::lbfgs_parameter_t lbfgs_params;

        Eigen::Matrix3Xd points;
        Eigen::VectorXd times;
        Eigen::Matrix3Xd gradByPoints;
        Eigen::VectorXd gradByTimes;
        Eigen::MatrixX3d partialGradByCoeffs;
        Eigen::VectorXd partialGradByTimes;

    private:
        static inline void forwardT(const Eigen::VectorXd &tau,
                                    Eigen::VectorXd &T)
        {
            const int sizeTau = tau.size();
            T.resize(sizeTau);
            for (int i = 0; i < sizeTau; i++)
            {
                T(i) = tau(i) > 0.0
                           ? ((0.5 * tau(i) + 1.0) * tau(i) + 1.0)
                           : 1.0 / ((0.5 * tau(i) - 1.0) * tau(i) + 1.0);
            }
            return;
        }

        template <typename EIGENVEC>
        static inline void backwardT(const Eigen::VectorXd &T,
                                     EIGENVEC &tau)
        {
            const int sizeT = T.size();
            tau.resize(sizeT);
            for (int i = 0; i < sizeT; i++)
            {
                tau(i) = T(i) > 1.0
                             ? (sqrt(2.0 * T(i) - 1.0) - 1.0)
                             : (1.0 - sqrt(2.0 / T(i) - 1.0));
            }

            return;
        }

        template <typename EIGENVEC>
        static inline void backwardGradT(const Eigen::VectorXd &tau,
                                         const Eigen::VectorXd &gradT,
                                         EIGENVEC &gradTau)
        {
            const int sizeTau = tau.size();
            gradTau.resize(sizeTau);
            double denSqrt;
            for (int i = 0; i < sizeTau; i++)
            {
                if (tau(i) > 0)
                {
                    gradTau(i) = gradT(i) * (tau(i) + 1.0);
                }
                else
                {
                    denSqrt = (0.5 * tau(i) - 1.0) * tau(i) + 1.0;
                    gradTau(i) = gradT(i) * (1.0 - tau(i)) / (denSqrt * denSqrt);
                }
            }

            return;
        }

        static inline void forwardP(const Eigen::VectorXd &xi,
                                    const Eigen::VectorXi &vIdx,
                                    const PolyhedraV &vPolys,
                                    Eigen::Matrix3Xd &P)
        {
            const int sizeP = vIdx.size();
            P.resize(3, sizeP);
            Eigen::VectorXd q;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();
                q = xi.segment(j, k).normalized().head(k - 1);
                P.col(i) = vPolys[l].rightCols(k - 1) * q.cwiseProduct(q) +
                           vPolys[l].col(0);
            }
            return;
        }

        static inline double costTinyNLS(void *ptr,
                                         const Eigen::VectorXd &xi,
                                         Eigen::VectorXd &gradXi)
        {
            const int n = xi.size();
            const Eigen::Matrix3Xd &ovPoly = *(Eigen::Matrix3Xd *)ptr;

            const double sqrNormXi = xi.squaredNorm();
            const double invNormXi = 1.0 / sqrt(sqrNormXi);
            const Eigen::VectorXd unitXi = xi * invNormXi;
            const Eigen::VectorXd r = unitXi.head(n - 1);
            const Eigen::Vector3d delta = ovPoly.rightCols(n - 1) * r.cwiseProduct(r) +
                                          ovPoly.col(1) - ovPoly.col(0);

            double cost = delta.squaredNorm();
            gradXi.head(n - 1) = (ovPoly.rightCols(n - 1).transpose() * (2 * delta)).array() *
                                 r.array() * 2.0;
            gradXi(n - 1) = 0.0;
            gradXi = (gradXi - unitXi.dot(gradXi) * unitXi).eval() * invNormXi;

            const double sqrNormViolation = sqrNormXi - 1.0;
            if (sqrNormViolation > 0.0)
            {
                double c = sqrNormViolation * sqrNormViolation;
                const double dc = 3.0 * c;
                c *= sqrNormViolation;
                cost += c;
                gradXi += dc * 2.0 * xi;
            }

            return cost;
        }

        template <typename EIGENVEC>
        static inline void backwardP(const Eigen::Matrix3Xd &P,
                                     const Eigen::VectorXi &vIdx,
                                     const PolyhedraV &vPolys,
                                     EIGENVEC &xi)
        {
            const int sizeP = P.cols();

            double minSqrD;
            lbfgs::lbfgs_parameter_t tiny_nls_params;
            tiny_nls_params.past = 0;
            tiny_nls_params.delta = 1.0e-5;
            tiny_nls_params.g_epsilon = FLT_EPSILON;
            tiny_nls_params.max_iterations = 128;

            Eigen::Matrix3Xd ovPoly;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();

                ovPoly.resize(3, k + 1);
                ovPoly.col(0) = P.col(i);
                ovPoly.rightCols(k) = vPolys[l];
                Eigen::VectorXd x(k);
                x.setConstant(sqrt(1.0 / k));
                lbfgs::lbfgs_optimize(x,
                                      minSqrD,
                                      &GCOPTER_PolytopeSFC::costTinyNLS,
                                      nullptr,
                                      nullptr,
                                      &ovPoly,
                                      tiny_nls_params);

                xi.segment(j, k) = x;
            }

            return;
        }

        template <typename EIGENVEC>
        static inline void backwardGradP(const Eigen::VectorXd &xi,
                                         const Eigen::VectorXi &vIdx,
                                         const PolyhedraV &vPolys,
                                         const Eigen::Matrix3Xd &gradP,
                                         EIGENVEC &gradXi)
        {
            const int sizeP = vIdx.size();
            gradXi.resize(xi.size());

            double normInv;
            Eigen::VectorXd q, gradQ, unitQ;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();
                q = xi.segment(j, k);
                normInv = 1.0 / q.norm();
                unitQ = q * normInv;
                gradQ.resize(k);
                gradQ.head(k - 1) = (vPolys[l].rightCols(k - 1).transpose() * gradP.col(i)).array() *
                                    unitQ.head(k - 1).array() * 2.0;
                gradQ(k - 1) = 0.0;
                gradXi.segment(j, k) = (gradQ - unitQ * unitQ.dot(gradQ)) * normInv;
            }

            return;
        }

        template <typename EIGENVEC>
        static inline void normRetrictionLayer(const Eigen::VectorXd &xi,
                                               const Eigen::VectorXi &vIdx,
                                               const PolyhedraV &vPolys,
                                               double &cost,
                                               EIGENVEC &gradXi)
        {
            const int sizeP = vIdx.size();
            gradXi.resize(xi.size());

            double sqrNormQ, sqrNormViolation, c, dc;
            Eigen::VectorXd q;
            for (int i = 0, j = 0, k; i < sizeP; i++, j += k)
            {
                k = vPolys[vIdx(i)].cols();

                q = xi.segment(j, k);
                sqrNormQ = q.squaredNorm();
                sqrNormViolation = sqrNormQ - 1.0;
                if (sqrNormViolation > 0.0)
                {
                    c = sqrNormViolation * sqrNormViolation;
                    dc = 3.0 * c;
                    c *= sqrNormViolation;
                    cost += c;
                    gradXi.segment(j, k) += dc * 2.0 * q;
                }
            }

            return;
        }

        static inline bool smoothedL1(const double &x,
                                      const double &mu,
                                      double &f,
                                      double &df)
        {
            if (x < 0.0)
            {
                return false;
            }
            else if (x > mu)
            {
                f = x - 0.5 * mu;
                df = 1.0;
                return true;
            }
            else
            {
                const double xdmu = x / mu;
                const double sqrxdmu = xdmu * xdmu;
                const double mumxd2 = mu - 0.5 * x;
                f = mumxd2 * sqrxdmu * xdmu;
                df = sqrxdmu * ((-0.5) * xdmu + 3.0 * mumxd2 / mu);
                return true;
            }
        }

        // magnitudeBounds = [v_max, omg_max, theta_max, thrust_min, thrust_max]^T
        // penaltyWeights = [pos_weight, vel_weight, omg_weight, theta_weight, thrust_weight]^T
        // physicalParams = [vehicle_mass, gravitational_acceleration, horitonral_drag_coeff,
        //                   vertical_drag_coeff, parasitic_drag_coeff, speed_smooth_factor]^T
        static inline void attachPenaltyFunctional(const Eigen::VectorXd &T,
                                                   const Eigen::MatrixX3d &coeffs,
                                                   const Eigen::VectorXi &hIdx,
                                                   const PolyhedraH &hPolys,
                                                   const double &smoothFactor,
                                                   const int &integralResolution,
                                                   const Eigen::VectorXd &magnitudeBounds,
                                                   const Eigen::VectorXd &penaltyWeights,
                                                   flatness::FlatnessMap &flatMap,
                                                   double &cost,
                                                   Eigen::VectorXd &gradT,
                                                   Eigen::MatrixX3d &gradC)
        {
            const double velSqrMax = magnitudeBounds(0) * magnitudeBounds(0);
            const double omgSqrMax = magnitudeBounds(1) * magnitudeBounds(1);
            const double thetaMax = magnitudeBounds(2);
            const double thrustMean = 0.5 * (magnitudeBounds(3) + magnitudeBounds(4));
            const double thrustRadi = 0.5 * fabs(magnitudeBounds(4) - magnitudeBounds(3));
            const double thrustSqrRadi = thrustRadi * thrustRadi;

            const double weightPos = penaltyWeights(0);
            const double weightVel = penaltyWeights(1);
            const double weightOmg = penaltyWeights(2);
            const double weightTheta = penaltyWeights(3);
            const double weightThrust = penaltyWeights(4);

            Eigen::Vector3d pos, vel, acc, jer, sna;
            Eigen::Vector3d totalGradPos, totalGradVel, totalGradAcc, totalGradJer;
            double totalGradPsi, totalGradPsiD;
            double thr, cos_theta;
            Eigen::Vector4d quat;
            Eigen::Vector3d omg;
            double gradThr;
            Eigen::Vector4d gradQuat;
            Eigen::Vector3d gradPos, gradVel, gradOmg;

            double step, alpha;
            double s1, s2, s3, s4, s5;
            Eigen::Matrix<double, 6, 1> beta0, beta1, beta2, beta3, beta4;
            Eigen::Vector3d outerNormal;
            int K, L;
            double violaPos, violaVel, violaOmg, violaTheta, violaThrust;
            double violaPosPenaD, violaVelPenaD, violaOmgPenaD, violaThetaPenaD, violaThrustPenaD;
            double violaPosPena, violaVelPena, violaOmgPena, violaThetaPena, violaThrustPena;
            double node, pena;

            const int pieceNum = T.size();
            const double integralFrac = 1.0 / integralResolution;
            for (int i = 0; i < pieceNum; i++)
            {
                const Eigen::Matrix<double, 6, 3> &c = coeffs.block<6, 3>(i * 6, 0);
                step = T(i) * integralFrac;
                for (int j = 0; j <= integralResolution; j++)
                {
                    s1 = j * step;
                    s2 = s1 * s1;
                    s3 = s2 * s1;
                    s4 = s2 * s2;
                    s5 = s4 * s1;
                    beta0(0) = 1.0, beta0(1) = s1, beta0(2) = s2, beta0(3) = s3, beta0(4) = s4, beta0(5) = s5;
                    beta1(0) = 0.0, beta1(1) = 1.0, beta1(2) = 2.0 * s1, beta1(3) = 3.0 * s2, beta1(4) = 4.0 * s3, beta1(5) = 5.0 * s4;
                    beta2(0) = 0.0, beta2(1) = 0.0, beta2(2) = 2.0, beta2(3) = 6.0 * s1, beta2(4) = 12.0 * s2, beta2(5) = 20.0 * s3;
                    beta3(0) = 0.0, beta3(1) = 0.0, beta3(2) = 0.0, beta3(3) = 6.0, beta3(4) = 24.0 * s1, beta3(5) = 60.0 * s2;
                    beta4(0) = 0.0, beta4(1) = 0.0, beta4(2) = 0.0, beta4(3) = 0.0, beta4(4) = 24.0, beta4(5) = 120.0 * s1;
                    pos = c.transpose() * beta0;
                    vel = c.transpose() * beta1;
                    acc = c.transpose() * beta2;
                    jer = c.transpose() * beta3;
                    sna = c.transpose() * beta4;

                    flatMap.forward(vel, acc, jer, 0.0, 0.0, thr, quat, omg);

                    violaVel = vel.squaredNorm() - velSqrMax;
                    violaOmg = omg.squaredNorm() - omgSqrMax;
                    cos_theta = 1.0 - 2.0 * (quat(1) * quat(1) + quat(2) * quat(2));
                    violaTheta = acos(cos_theta) - thetaMax;
                    violaThrust = (thr - thrustMean) * (thr - thrustMean) - thrustSqrRadi;

                    gradThr = 0.0;
                    gradQuat.setZero();
                    gradPos.setZero(), gradVel.setZero(), gradOmg.setZero();
                    pena = 0.0;

                    L = hIdx(i);
                    K = hPolys[L].rows();
                    for (int k = 0; k < K; k++)
                    {
                        outerNormal = hPolys[L].block<1, 3>(k, 0);
                        violaPos = outerNormal.dot(pos) + hPolys[L](k, 3);
                        if (smoothedL1(violaPos, smoothFactor, violaPosPena, violaPosPenaD))
                        {
                            gradPos += weightPos * violaPosPenaD * outerNormal;
                            pena += weightPos * violaPosPena;
                        }
                    }

                    if (smoothedL1(violaVel, smoothFactor, violaVelPena, violaVelPenaD))
                    {
                        gradVel += weightVel * violaVelPenaD * 2.0 * vel;
                        pena += weightVel * violaVelPena;
                    }

                    if (smoothedL1(violaOmg, smoothFactor, violaOmgPena, violaOmgPenaD))
                    {
                        gradOmg += weightOmg * violaOmgPenaD * 2.0 * omg;
                        pena += weightOmg * violaOmgPena;
                    }

                    if (smoothedL1(violaTheta, smoothFactor, violaThetaPena, violaThetaPenaD))
                    {
                        gradQuat += weightTheta * violaThetaPenaD /
                                    sqrt(1.0 - cos_theta * cos_theta) * 4.0 *
                                    Eigen::Vector4d(0.0, quat(1), quat(2), 0.0);
                        pena += weightTheta * violaThetaPena;
                    }

                    if (smoothedL1(violaThrust, smoothFactor, violaThrustPena, violaThrustPenaD))
                    {
                        gradThr += weightThrust * violaThrustPenaD * 2.0 * (thr - thrustMean);
                        pena += weightThrust * violaThrustPena;
                    }

                    flatMap.backward(gradPos, gradVel, gradThr, gradQuat, gradOmg,
                                     totalGradPos, totalGradVel, totalGradAcc, totalGradJer,
                                     totalGradPsi, totalGradPsiD);

                    node = (j == 0 || j == integralResolution) ? 0.5 : 1.0;
                    alpha = j * integralFrac;
                    gradC.block<6, 3>(i * 6, 0) += (beta0 * totalGradPos.transpose() +
                                                    beta1 * totalGradVel.transpose() +
                                                    beta2 * totalGradAcc.transpose() +
                                                    beta3 * totalGradJer.transpose()) *
                                                   node * step;
                    gradT(i) += (totalGradPos.dot(vel) +
                                 totalGradVel.dot(acc) +
                                 totalGradAcc.dot(jer) +
                                 totalGradJer.dot(sna)) *
                                    alpha * node * step +
                                node * integralFrac * pena;
                    cost += node * step * pena;
                }
            }

            return;
        }

        static inline double costFunctional(void *ptr,
                                            const Eigen::VectorXd &x,
                                            Eigen::VectorXd &g)
        {
            GCOPTER_PolytopeSFC &obj = *(GCOPTER_PolytopeSFC *)ptr;
            const int dimTau = obj.temporalDim;
            const int dimXi = obj.spatialDim;
            const double weightT = obj.rho;
            Eigen::Map<const Eigen::VectorXd> tau(x.data(), dimTau);
            Eigen::Map<const Eigen::VectorXd> xi(x.data() + dimTau, dimXi);
            Eigen::Map<Eigen::VectorXd> gradTau(g.data(), dimTau);
            Eigen::Map<Eigen::VectorXd> gradXi(g.data() + dimTau, dimXi);

            forwardT(tau, obj.times);
            forwardP(xi, obj.vPolyIdx, obj.vPolytopes, obj.points);

            double cost;
            obj.minco.setParameters(obj.points, obj.times);
            obj.minco.getEnergy(cost);
            obj.minco.getEnergyPartialGradByCoeffs(obj.partialGradByCoeffs);
            obj.minco.getEnergyPartialGradByTimes(obj.partialGradByTimes);

            attachPenaltyFunctional(obj.times, obj.minco.getCoeffs(),
                                    obj.hPolyIdx, obj.hPolytopes,
                                    obj.smoothEps, obj.integralRes,
                                    obj.magnitudeBd, obj.penaltyWt, obj.flatmap,
                                    cost, obj.partialGradByTimes, obj.partialGradByCoeffs);

            obj.minco.propogateGrad(obj.partialGradByCoeffs, obj.partialGradByTimes,
                                    obj.gradByPoints, obj.gradByTimes);

            cost += weightT * obj.times.sum();
            obj.gradByTimes.array() += weightT;

            backwardGradT(tau, obj.gradByTimes, gradTau);
            backwardGradP(xi, obj.vPolyIdx, obj.vPolytopes, obj.gradByPoints, gradXi);
            normRetrictionLayer(xi, obj.vPolyIdx, obj.vPolytopes, cost, gradXi);

            return cost;
        }

        static inline double costDistance(void *ptr,
                                          const Eigen::VectorXd &xi,
                                          Eigen::VectorXd &gradXi)
        {
            void **dataPtrs = (void **)ptr;
            const double &dEps = *((const double *)(dataPtrs[0]));
            const Eigen::Vector3d &ini = *((const Eigen::Vector3d *)(dataPtrs[1]));
            const Eigen::Vector3d &fin = *((const Eigen::Vector3d *)(dataPtrs[2]));
            const PolyhedraV &vPolys = *((PolyhedraV *)(dataPtrs[3]));

            double cost = 0.0;
            const int overlaps = vPolys.size() / 2;

            Eigen::Matrix3Xd gradP = Eigen::Matrix3Xd::Zero(3, overlaps);
            Eigen::Vector3d a, b, d;
            Eigen::VectorXd r;
            double smoothedDistance;
            for (int i = 0, j = 0, k = 0; i <= overlaps; i++, j += k)
            {
                a = i == 0 ? ini : b;
                if (i < overlaps)
                {
                    k = vPolys[2 * i + 1].cols();
                    Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                    r = q.normalized().head(k - 1);
                    b = vPolys[2 * i + 1].rightCols(k - 1) * r.cwiseProduct(r) +
                        vPolys[2 * i + 1].col(0);
                }
                else
                {
                    b = fin;
                }

                d = b - a;
                smoothedDistance = sqrt(d.squaredNorm() + dEps);
                cost += smoothedDistance;

                if (i < overlaps)
                {
                    gradP.col(i) += d / smoothedDistance;
                }
                if (i > 0)
                {
                    gradP.col(i - 1) -= d / smoothedDistance;
                }
            }

            Eigen::VectorXd unitQ;
            double sqrNormQ, invNormQ, sqrNormViolation, c, dc;
            for (int i = 0, j = 0, k; i < overlaps; i++, j += k)
            {
                k = vPolys[2 * i + 1].cols();
                Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                Eigen::Map<Eigen::VectorXd> gradQ(gradXi.data() + j, k);
                sqrNormQ = q.squaredNorm();
                invNormQ = 1.0 / sqrt(sqrNormQ);
                unitQ = q * invNormQ;
                gradQ.head(k - 1) = (vPolys[2 * i + 1].rightCols(k - 1).transpose() * gradP.col(i)).array() *
                                    unitQ.head(k - 1).array() * 2.0;
                gradQ(k - 1) = 0.0;
                gradQ = (gradQ - unitQ * unitQ.dot(gradQ)).eval() * invNormQ;

                sqrNormViolation = sqrNormQ - 1.0;
                if (sqrNormViolation > 0.0)
                {
                    c = sqrNormViolation * sqrNormViolation;
                    dc = 3.0 * c;
                    c *= sqrNormViolation;
                    cost += c;
                    gradQ += dc * 2.0 * q;
                }
            }

            return cost;
        }

        static inline void getShortestPath(const Eigen::Vector3d &ini,
                                           const Eigen::Vector3d &fin,
                                           const PolyhedraV &vPolys,
                                           const double &smoothD,
                                           Eigen::Matrix3Xd &path)
        {
            const int overlaps = vPolys.size() / 2;
            Eigen::VectorXi vSizes(overlaps);
            for (int i = 0; i < overlaps; i++)
            {
                vSizes(i) = vPolys[2 * i + 1].cols();
            }
            Eigen::VectorXd xi(vSizes.sum());
            for (int i = 0, j = 0; i < overlaps; i++)
            {
                xi.segment(j, vSizes(i)).setConstant(sqrt(1.0 / vSizes(i)));
                j += vSizes(i);
            }

            double minDistance;
            void *dataPtrs[4];
            dataPtrs[0] = (void *)(&smoothD);
            dataPtrs[1] = (void *)(&ini);
            dataPtrs[2] = (void *)(&fin);
            dataPtrs[3] = (void *)(&vPolys);
            lbfgs::lbfgs_parameter_t shortest_path_params;
            shortest_path_params.past = 3;
            shortest_path_params.delta = 1.0e-3;
            shortest_path_params.g_epsilon = 1.0e-5;

            lbfgs::lbfgs_optimize(xi,
                                  minDistance,
                                  &GCOPTER_PolytopeSFC::costDistance,
                                  nullptr,
                                  nullptr,
                                  dataPtrs,
                                  shortest_path_params);

            path.resize(3, overlaps + 2);
            path.leftCols<1>() = ini;
            path.rightCols<1>() = fin;
            Eigen::VectorXd r;
            for (int i = 0, j = 0, k; i < overlaps; i++, j += k)
            {
                k = vPolys[2 * i + 1].cols();
                Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                r = q.normalized().head(k - 1);
                path.col(i + 1) = vPolys[2 * i + 1].rightCols(k - 1) * r.cwiseProduct(r) +
                                  vPolys[2 * i + 1].col(0);
            }

            return;
        }

        static inline bool processCorridor(const PolyhedraH &hPs,
                                           PolyhedraV &vPs)
        {
            const int sizeCorridor = hPs.size() - 1;

            vPs.clear();
            vPs.reserve(2 * sizeCorridor + 1);

            int nv;
            PolyhedronH curIH;
            PolyhedronV curIV, curIOB;
            for (int i = 0; i < sizeCorridor; i++)
            {
                if (!geo_utils::enumerateVs(hPs[i], curIV))
                {
                    return false;
                }
                nv = curIV.cols();
                curIOB.resize(3, nv);
                curIOB.col(0) = curIV.col(0);
                curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
                vPs.push_back(curIOB);

                curIH.resize(hPs[i].rows() + hPs[i + 1].rows(), 4);
                curIH.topRows(hPs[i].rows()) = hPs[i];
                curIH.bottomRows(hPs[i + 1].rows()) = hPs[i + 1];
                if (!geo_utils::enumerateVs(curIH, curIV))
                {
                    return false;
                }
                nv = curIV.cols();
                curIOB.resize(3, nv);
                curIOB.col(0) = curIV.col(0);
                curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
                vPs.push_back(curIOB);
            }

            if (!geo_utils::enumerateVs(hPs.back(), curIV))
            {
                return false;
            }
            nv = curIV.cols();
            curIOB.resize(3, nv);
            curIOB.col(0) = curIV.col(0);
            curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
            vPs.push_back(curIOB);

            return true;
        }

        static inline void setInitial(const Eigen::Matrix3Xd &path,
                                      const double &speed,
                                      const Eigen::VectorXi &intervalNs,
                                      Eigen::Matrix3Xd &innerPoints,
                                      Eigen::VectorXd &timeAlloc)
        {
            const int sizeM = intervalNs.size();
            const int sizeN = intervalNs.sum();
            innerPoints.resize(3, sizeN - 1);
            timeAlloc.resize(sizeN);

            Eigen::Vector3d a, b, c;
            for (int i = 0, j = 0, k = 0, l; i < sizeM; i++)
            {
                l = intervalNs(i);
                a = path.col(i);
                b = path.col(i + 1);
                c = (b - a) / l;
                timeAlloc.segment(j, l).setConstant(c.norm() / speed);
                j += l;
                for (int m = 0; m < l; m++)
                {
                    if (i > 0 || m > 0)
                    {
                        innerPoints.col(k++) = a + c * m;
                    }
                }
            }
        }

    public:
        // magnitudeBounds = [v_max, omg_max, theta_max, thrust_min, thrust_max]^T
        // penaltyWeights = [pos_weight, vel_weight, omg_weight, theta_weight, thrust_weight]^T
        // physicalParams = [vehicle_mass, gravitational_acceleration, horitonral_drag_coeff,
        //                   vertical_drag_coeff, parasitic_drag_coeff, speed_smooth_factor]^T
        inline bool setup(const double &timeWeight,
                          const Eigen::Matrix3d &initialPVA,
                          const Eigen::Matrix3d &terminalPVA,
                          const PolyhedraH &safeCorridor,
                          const double &lengthPerPiece,
                          const double &smoothingFactor,
                          const int &integralResolution,
                          const Eigen::VectorXd &magnitudeBounds,
                          const Eigen::VectorXd &penaltyWeights,
                          const Eigen::VectorXd &physicalParams)
        {
            rho = timeWeight;
            headPVA = initialPVA;
            tailPVA = terminalPVA;

            hPolytopes = safeCorridor;
            for (size_t i = 0; i < hPolytopes.size(); i++)
            {
                const Eigen::ArrayXd norms =
                    hPolytopes[i].leftCols<3>().rowwise().norm();
                hPolytopes[i].array().colwise() /= norms;
            }
            if (!processCorridor(hPolytopes, vPolytopes))
            {
                return false;
            }

            polyN = hPolytopes.size();
            smoothEps = smoothingFactor;
            integralRes = integralResolution;
            magnitudeBd = magnitudeBounds;
            penaltyWt = penaltyWeights;
            physicalPm = physicalParams;
            allocSpeed = magnitudeBd(0) * 3.0;

            getShortestPath(headPVA.col(0), tailPVA.col(0),
                            vPolytopes, smoothEps, shortPath);
            const Eigen::Matrix3Xd deltas = shortPath.rightCols(polyN) - shortPath.leftCols(polyN);
            pieceIdx = (deltas.colwise().norm() / lengthPerPiece).cast<int>().transpose();
            pieceIdx.array() += 1;
            pieceN = pieceIdx.sum();

            temporalDim = pieceN;
            spatialDim = 0;
            vPolyIdx.resize(pieceN - 1);
            hPolyIdx.resize(pieceN);
            for (int i = 0, j = 0, k; i < polyN; i++)
            {
                k = pieceIdx(i);
                for (int l = 0; l < k; l++, j++)
                {
                    if (l < k - 1)
                    {
                        vPolyIdx(j) = 2 * i;
                        spatialDim += vPolytopes[2 * i].cols();
                    }
                    else if (i < polyN - 1)
                    {
                        vPolyIdx(j) = 2 * i + 1;
                        spatialDim += vPolytopes[2 * i + 1].cols();
                    }
                    hPolyIdx(j) = i;
                }
            }

            // Setup for MINCO_S3NU, FlatnessMap, and L-BFGS solver
            minco.setConditions(headPVA, tailPVA, pieceN);
            flatmap.reset(physicalPm(0), physicalPm(1), physicalPm(2),
                          physicalPm(3), physicalPm(4), physicalPm(5));

            // Allocate temp variables
            points.resize(3, pieceN - 1);
            times.resize(pieceN);
            gradByPoints.resize(3, pieceN - 1);
            gradByTimes.resize(pieceN);
            partialGradByCoeffs.resize(6 * pieceN, 3);
            partialGradByTimes.resize(pieceN);

            return true;
        }

        inline double optimize(Trajectory<5> &traj,
                               const double &relCostTol)
        {
            Eigen::VectorXd x(temporalDim + spatialDim);
            Eigen::Map<Eigen::VectorXd> tau(x.data(), temporalDim);
            Eigen::Map<Eigen::VectorXd> xi(x.data() + temporalDim, spatialDim);

            setInitial(shortPath, allocSpeed, pieceIdx, points, times);
            backwardT(times, tau);
            backwardP(points, vPolyIdx, vPolytopes, xi);

            double minCostFunctional;
            lbfgs_params.mem_size = 256;
            lbfgs_params.past = 3;
            lbfgs_params.min_step = 1.0e-32;
            lbfgs_params.g_epsilon = 0.0;
            lbfgs_params.delta = relCostTol;

            int ret = lbfgs::lbfgs_optimize(x,
                                            minCostFunctional,
                                            &GCOPTER_PolytopeSFC::costFunctional,
                                            nullptr,
                                            nullptr,
                                            this,
                                            lbfgs_params);

            if (ret >= 0)
            {
                forwardT(tau, times);
                forwardP(xi, vPolyIdx, vPolytopes, points);
                minco.setParameters(points, times);
                minco.getTrajectory(traj);
            }
            else
            {
                traj.clear();
                minCostFunctional = INFINITY;
                std::cout << "Optimization Failed: "
                          << lbfgs::lbfgs_strerror(ret)
                          << std::endl;
            }

            return minCostFunctional;
        }
    };

}

#endif


/**
 * @file 	SteinerTreeHeuristic.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 14, 2017
 */

#include <sstream>
#include <boost/heap/d_ary_heap.hpp>
#include "SteinerTreeHeuristic.h"

using std::pair;

/*
 * Data that is stored in one node of the heap. Contains an integer and a double.
 * Comparisons are made by the double, smaller has higher priority. The int is
 * used for the vertex index and the double for its key during the algorithm
 */
struct heap_data {

	int index;
	weight_type key;

    heap_data(int i, weight_type k): index(i), key(k) {
    }

    bool operator<(heap_data const & rhs) const {
        return key > rhs.key;
    }
};

/*
 * Implementation of the improved Shortest-Path-Heuristic. A 4-ary-heap from
 * boost is used and keys are never decreased, but instead new nodes are pushed
 * whenever a better path is found. The resulting tree is stored implicitly
 */
double SteinerTreeHeuristic::compute_steiner_tree(const CSR_Graph& g, int num_vertices,
		int root, vector<int>& tree_preds, const vector<int>& terminals) {

	assert((int) tree_preds.size() == num_vertices);

	// terminals are scanned
	int num_terminals = terminals.size();
	vector<bool> is_terminal(num_vertices, false);
	for (int i = 0; i < num_terminals; ++i) {
		assert(terminals[i] < num_vertices);
		is_terminal[terminals[i]] = true;
	}

	assert(is_terminal[root]);

	heap::d_ary_heap<heap_data, heap::arity<4>> heap;

	// this vector is used to store the info about predecessor and the respective
	// edge together in order to avoid cache misses
	vector<pair<int,weight_type>> preds(num_vertices);
	preds[root] = std::make_pair(root, 0);

	// we expect the tree vector to be filled with -1 already
	for (int i = 0; i < num_vertices; ++i) {
		assert(tree_preds[i] == -1);
	}

	// initialization of distances. all but the root vertex have infinity
	vector<weight_type> dist(num_vertices, std::numeric_limits<weight_type>::infinity());
	dist[root] = 0;
	tree_preds[root] = root;

	heap.push(heap_data(root, 0));

	// this is just boost syntax so that we can access weights and indices later
	property_map<CSR_Graph, edge_weight_t>::const_type weights = get(edge_weight, g);
	property_map<CSR_Graph, vertex_index_t>::const_type index = get(vertex_index, g);

	int connected_terminals = 1;
	weight_type tree_cost = 0;

	// here the actual algorithm starts
	while (connected_terminals < num_terminals) {

		assert(!heap.empty());	//heap should never be empty before tree contains all terminals

		int min_idx = heap.top().index;
		weight_type min_key = heap.top().key;

		heap.pop();

		if (dist[min_idx] < min_key) continue;

		// if we scan a terminal, all vertices on shortest path to subtree
		// are added to heap with weight 0 and included into the tree
		if (is_terminal[min_idx] && tree_preds[min_idx] == -1) {

			++connected_terminals;

			min_key = 0;
			int next_vertex = preds[min_idx].first;
			tree_preds[min_idx] = next_vertex;

			tree_cost += preds[min_idx].second;

			// here all vertices on path from new terminal to tree are added again with key=0
			while (tree_preds[next_vertex] == -1) {

				heap.push(heap_data(next_vertex, 0));
				dist[next_vertex] = 0;
				tree_preds[next_vertex] = preds[next_vertex].first;

				tree_cost += preds[next_vertex].second;

				next_vertex = preds[next_vertex].first;
			}
		}

		typename graph_traits<CSR_Graph>::out_edge_iterator it, it_end;

		// this is basically dijkstra with pushes instead of decreasekey operations
		for (tie(it,it_end) = out_edges(*(vertices(g).first+min_idx),g); it != it_end; ++it) {

			int v_target = index[target(*it, g)];

			// we only consider vertices which have not been added to the tree yet
			if (tree_preds[v_target] != -1) continue;

			if (min_key + weights[*it] < dist[v_target]) {
				// distance is updated
				dist[v_target] = min_key + weights[*it];
				preds[v_target].first = min_idx;
				preds[v_target].second = weights[*it];
				heap.push(heap_data(v_target, dist[v_target]));
			}
		}
	}

	return tree_cost;
}

/* note: this method only works if tree_preds actually represents a tree.
 * If not sure, use function testTree(...)
 */
string SteinerTreeHeuristic::print_tree(const CSR_Graph& g, int num_vertices, int root,
		vector<int>& tree_preds, vector<int>& terminals) {

	std::stringstream stream;
	vector<bool> visited(num_vertices, false);

	// we iterate through the tree going from each terminal until we arrive at root
	for (unsigned int i = 0; i < terminals.size(); ++i) {
		int curr = terminals[i];
		int edge_count = 0;
		while (curr != root && !visited[curr]) {
			++edge_count;

			// every 50 lines a linebreak is added for the sake of readability
			if (edge_count % 50 == 0) stream << std::endl;
			stream << "(" << curr << "," << tree_preds[curr] << ") ";

			visited[curr] = true;
			curr = tree_preds[curr];
		}
	}

	return stream.str();
}

bool SteinerTreeHeuristic::test_tree(const CSR_Graph& g, int num_vertices, int root,
	vector<int>& tree_preds, vector<int>& terminals) {

	vector<bool> visited(num_vertices, false);

	// we iterate through the tree going from each terminal until we arrive at
	// the root or a vertex that we already visited
	for (unsigned int i = 0; i < terminals.size(); ++i) {
		int curr = terminals[i];
		int vertex_count = 1;

		while (curr != root && !visited[curr] && vertex_count != num_vertices) {
			// if one terminal is not connected to root, result is false
			if (curr == -1) return false;
			visited[curr] = true;
			curr = tree_preds[curr];
			++vertex_count;
		}

		// if a cycle is found, result is false
		if (vertex_count == num_vertices || curr == -1){
			return false;
		}
	}

	return true;
}


#pragma once
#include <Eigen/Dense>

struct InternalBoundingBox
{
	/// The position of the box's corner that has the least coordinate value in all axes.
	Eigen::Vector3i pos;
	/// The extents of the cube.
	Eigen::Vector3i span;

	static bool IsPointInCube(const InternalBoundingBox& cube, Eigen::Vector3i point)
	{
		bool xIntersects = cube.pos.x() <= point.x() && cube.pos.x() + cube.span.x() > point.x();
		bool yIntersects = cube.pos.y() <= point.y() && cube.pos.y() + cube.span.y() > point.y();
		bool zIntersects = cube.pos.z() <= point.z() && cube.pos.z() + cube.span.z() > point.z();
		return xIntersects && yIntersects && zIntersects;
	}

	static bool CubesIntersect(const InternalBoundingBox& cube1, const InternalBoundingBox& cube2)
	{
		bool xIntersects = !(cube2.pos.x() + cube2.span.x() <= cube1.pos.x() || cube1.pos.x() + cube1.span.x() <= cube2.pos.x());
		bool yIntersects = !(cube2.pos.y() + cube2.span.y() <= cube1.pos.y() || cube1.pos.y() + cube1.span.y() <= cube2.pos.y());
		bool zIntersects = !(cube2.pos.z() + cube2.span.z() <= cube1.pos.z() || cube1.pos.z() + cube1.span.z() <= cube2.pos.z());
		return xIntersects && yIntersects && zIntersects;
	}

	static void SplitCube(const InternalBoundingBox& cube, InternalBoundingBox children[8])
	{
		Eigen::Vector3i newSpan = cube.span / 2;

		children[0].pos = { cube.pos.x(), cube.pos.y(), cube.pos.z() };
		children[0].span = newSpan;

		children[1].pos = { cube.pos.x(), cube.pos.y(), cube.pos.z() + newSpan.z() };
		children[1].span = newSpan;

		children[2].pos = { cube.pos.x(), cube.pos.y() + newSpan.y(), cube.pos.z() };
		children[2].span = newSpan;

		children[3].pos = { cube.pos.x(), cube.pos.y() + newSpan.y(), cube.pos.z() + newSpan.z() };
		children[3].span = newSpan;

		children[4].pos = { cube.pos.x() + newSpan.x(), cube.pos.y(), cube.pos.z() };
		children[4].span = newSpan;

		children[5].pos = { cube.pos.x() + newSpan.x(), cube.pos.y(), cube.pos.z() + newSpan.z() };
		children[5].span = newSpan;

		children[6].pos = { cube.pos.x() + newSpan.x(), cube.pos.y() + newSpan.y(), cube.pos.z() };
		children[6].span = newSpan;

		children[7].pos = { cube.pos.x() + newSpan.x(), cube.pos.y() + newSpan.y(), cube.pos.z() + newSpan.z() };
		children[7].span = newSpan;
	}

	friend std::ostream& operator<<(std::ostream& os, const InternalBoundingBox& cube)
	{
		os << '[' << cube.pos.x() << ", " << cube.pos.y() << ", " << cube.pos.z() << "] -> "
			<< '[' << cube.pos.x() + cube.span.x() - 1 << ", " << cube.pos.y() + cube.span.y() - 1 <<
			", " << cube.pos.z() + cube.span.z() - 1 << ']';
		return os;
	}
};


/*  $Id: conecutter.cpp 681 2011-03-23 21:17:32Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "conecutter.h"
#include "compositecutter.h" // for offsetCutter()
#include "numeric.h"

namespace ocl
{

ConeCutter::ConeCutter() {
    assert(0);
}

ConeCutter::ConeCutter(double d, double a, double l) {
    diameter = d;
    radius = d/2.0;
    angle = a;
    length = radius/tan(angle) + l;
    center_height = radius/tan(angle);
    xy_normal_length = radius;
    normal_length = 0.0;
}

double ConeCutter::height(double r) const {
    assert( tan(angle) > 0.0 ); // guard against division by zero
    return r/tan(angle);
}

double ConeCutter::width(double h) const {
    // grows from zero up to radius
    // above that (cutter shaft) return radius
    return (h<center_height) ? h*tan(angle) : radius ;
}

// ?? Ball-Cone-Bull ??
MillingCutter* ConeCutter::offsetCutter(double d) const {
    return new BallConeCutter(2*d,  diameter+2*d, angle) ;
}

// because this checks for contact with both the tip and the circular edge it is hard to move to the base-class
// we either hit the tip, when the slope of the plane is smaller than angle
// or when the slope is steep, the circular edge between the cone and the cylindrical shaft
bool ConeCutter::facetDrop(CLPoint &cl, const Triangle &t) const {
    bool result = false;
    Point normal = t.upNormal(); // facet surface normal
    if ( isZero_tol( normal.z ) )  // vertical surface
        return false;  //can't drop against vertical surface

    if ( (isZero_tol(normal.x)) && (isZero_tol(normal.y)) ) {  // horizontal plane special case
        CCPoint cc_tmp( cl.x, cl.y, t.p[0].z, FACET_TIP );  // so any vertex is at the correct height
        return cl.liftZ_if_inFacet(cc_tmp.z, cc_tmp, t);
    } else {
        // define plane containing facet
        // a*x + b*y + c*z + d = 0, so
        // d = -a*x - b*y - c*z, where  (a,b,c) = surface normal
        double a = normal.x;
        double b = normal.y;
        double c = normal.z;
        double d = - normal.dot(t.p[0]);
        normal.xyNormalize(); // make xy length of normal == 1.0
        // cylindrical contact point case
        // find the xy-coordinates of the cc-point
        CCPoint cyl_cc_tmp =  cl - radius*normal;
        cyl_cc_tmp.z = (1.0/c)*(-d-a*cyl_cc_tmp.x-b*cyl_cc_tmp.y);
        double cyl_cl_z = cyl_cc_tmp.z - length; // tip positioned here
        cyl_cc_tmp.type = FACET_CYL;

        // tip contact with facet
        CCPoint tip_cc_tmp(cl.x,cl.y,0.0);
        tip_cc_tmp.z = (1.0/c)*(-d-a*tip_cc_tmp.x-b*tip_cc_tmp.y);
        double tip_cl_z = tip_cc_tmp.z;
        tip_cc_tmp.type = FACET_TIP;

        result = result || cl.liftZ_if_inFacet( tip_cl_z, tip_cc_tmp, t);
        result = result || cl.liftZ_if_inFacet( cyl_cl_z, cyl_cc_tmp, t);
        return result;
    }
}

// cone sliced with vertical plane results in a hyperbola as the intersection curve
// find point where hyperbola and line slopes match
CC_CLZ_Pair ConeCutter::singleEdgeDropCanonical( const Point& u1, const Point& u2) const {
    double d = u1.y;
    double m = (u2.z-u1.z) / (u2.x-u1.x); // slope of edge
    // the outermost point on the cutter is at   xu = sqrt( R^2 - d^2 )
    double xu = sqrt( square(radius) - square(u1.y) );                  assert( xu <= radius );
    // max slope at xu is mu = (L/(R-R2)) * xu /(sqrt( xu^2 + d^2 ))
    double mu = (center_height/radius ) * xu / sqrt( square(xu) + square(d) ) ;
    bool hyperbola_case = (fabs(m) <= fabs(mu));
    // find contact point where slopes match, there are two cases:
    // 1) if abs(m) <= abs(mu)  we contact the curve at xp = sign(m) * sqrt( R^2 m^2 d^2 / (h^2 - R^2 m^2) )
    // 2) if abs(m) > abs(mu) there is contact with the circular edge at +/- xu
    double ccu;
    if ( hyperbola_case ) {
        ccu = sign(m) * sqrt( square(radius)*square(m)*square(d) / (square(length) -square(radius)*square(m) ) );
    } else {
        ccu = sign(m)*xu;
    }
    Point cc_tmp( ccu, d, 0.0); // cc-point in the XY plane
    cc_tmp.z_projectOntoEdge(u1,u2);
    double cl_z;
    if ( hyperbola_case ) {  // 1) zc = zp - Lc + (R - sqrt(xp^2 + d^2)) / tan(beta2)
        cl_z = cc_tmp.z - center_height + (radius-sqrt(square(ccu) + square(d)))/ tan(angle);
    } else {  // 2) zc = zp - Lc
        cl_z = cc_tmp.z - center_height; // case where we hit the edge of the cone
    }
    return CC_CLZ_Pair( ccu , cl_z);
}

bool ConeCutter::facetPush(const Fiber& fib, Interval& i,  const Triangle& t) const {
    // push two objects: tip, and base-circle
    bool result = false;
    if ( generalFacetPush( 0, 0, 0, fib, i, t) ) // TIP
        result = true;
    if ( generalFacetPush( 0, this->center_height, this->xy_normal_length , fib, i ,t) ) // BASE
        result = true;

    return result;
}

// cone is pushed along Fiber f into contact with edge p1-p2
bool ConeCutter::generalEdgePush(const Fiber& f, Interval& i,  const Point& p1, const Point& p2) const {
    bool result = false;

    if ( isZero_tol(p2.z-p1.z) ) // guard agains horizontal edge
        return result;
    assert( (p2.z-p1.z) != 0.0 );
    // idea: as the ITO-cone slides along the edge it will pierce a z-plane at the height of the fiber
    // the shaped of the pierced area is either a circle if the edge is steep
    // or a 'half-circle' + cone shape if the edge is shallow (ice-cream cone...)
    // we can now intersect this 2D shape with the fiber and get the CL-points.
    // how to get the CC-point? (point on edge closest to z-axis of cutter? closest to CL?)

    // this is where the ITO cone pierces the plane
    // edge-line: p1+t*(p2-p1) = zheight
    // => t = (zheight - p1)/ (p2-p1)
    double t_tip = (f.p1.z - p1.z) / (p2.z-p1.z);
    if (t_tip < 0.0 )
        t_tip = 0.0;
    Point p_tip = p1 + t_tip*(p2-p1);
    assert( isZero_tol( abs(p_tip.z-f.p1.z) ) ); // p_tip should be in plane of fiber

    // this is where the ITO cone base exits the plane
    double t_base = (f.p1.z+center_height - p1.z) / (p2.z-p1.z);
    Point p_base = p1 + t_base*(p2-p1);
    p_base.z = f.p1.z; // project to plane of fiber
    //std::cout << "(t0, t1) (" << t0 << " , " << t1 << ") \n";
    double L = (p_base-p_tip).xyNorm();

    if ( L <= radius ) { // this is where the ITO-slice is a circle
        // find intersection points, if any, between the fiber and the circle
        // fiber is f.p1 - f.p2
        // circle is centered at p_base and radius
        double d = p_base.xyDistanceToLine(f.p1, f.p2);
        if ( d <= radius ) {
            // we know there is an intersection point.
            // http://mathworld.wolfram.com/Circle-LineIntersection.html

            // subtract circle center, math is for circle centered at (0,0)
            double dx = f.p2.x - f.p1.x;
            double dy = f.p2.y - f.p1.y;
            double dr = sqrt( square(dx) + square(dy) );
            double det = (f.p1.x-p_base.x) * (f.p2.y-p_base.y) - (f.p2.x-p_base.x) * (f.p1.y-p_base.y);

            // intersection given by:
            //  x = det*dy +/- sign(dy) * dx * sqrt( r^2 dr^2 - det^2 )   / dr^2
            //  y = -det*dx +/- abs(dy)  * sqrt( r^2 dr^2 - det^2 )   / dr^2

            double discr = square(radius) * square(dr) - square(det);
            assert( discr > 0.0 ); // this means we have an intersection
            if ( discr == 0.0 ) { // tangent case
                double x_tang =  ( det*dy  )/ square(dr);
                double y_tang = -( det*dx  )/ square(dr);
                Point p_tang(x_tang+p_base.x, y_tang+p_base.y); // translate back from (0,0) system!
                double t_tang = f.tval( p_tang );
                if ( circle_CC( t_tang, p1, p2, f, i) )
                    result = true;
            } else {
                // two intersection points
                double x_pos = (  det*dy + sign(dy)* dx * sqrt( discr ) ) / square(dr);
                double y_pos = ( -det*dx + abs(dy)  * sqrt( discr ) ) / square(dr);
                Point p_pos(x_pos+p_base.x, y_pos+p_base.y);
                double t_pos = f.tval( p_pos );
                // the same with "-" sign:
                double x_neg = (  det*dy - sign(dy) * dx * sqrt( discr ) ) / square(dr);
                double y_neg = ( -det*dx - abs(dy)  * sqrt( discr ) ) / square(dr);
                Point p_neg(x_neg+p_base.x, y_neg+p_base.y);
                double t_neg = f.tval( p_neg );
                if ( circle_CC( t_pos, p1, p2, f, i) )
                    result = true;
                if ( circle_CC( t_neg, p1, p2, f, i) )
                    result = true;
            }
        }
        return result;
    } else {
        // ITO-slice is cone + half-circle
        // lines from p_tip to tangent points
        assert( L > radius );
        // http://mathworld.wolfram.com/CircleTangentLine.html
        // circle centered at x0, y0, radius a
        // tangent through (0,0)
        // t = +/- acos(  -a*x0 +/- y0*sqrt(x0^2+y0^2-a^2) / (x0^2+y0^2) )
        // translate so p_mid is at (0,0)
        //Point c = p_base - p_mid;
        //double cos1 = (-radius*c.x + c.y*sqrt(square(c.x)+square(c.y)+square(radius)) )/ (square(c.x) + square(c.y) );
        //double cos2 = (-radius*c.x - c.y*sqrt(square(c.x)+square(c.y)+square(radius)) )/ (square(c.x) + square(c.y) );

        return result;
    }
}

// t is a position along the fiber
// p1-p2 is the edge
// Interval& i is updated
bool ConeCutter::circle_CC( double t, const Point& p1, const Point& p2, const Fiber& f, Interval& i) const {
    // cone base circle is center_height above fiber
    double t_cc = (f.p1.z+center_height - p1.z) / (p2.z-p1.z);
    CCPoint cc_tmp = p1 + t_cc*(p2-p1); // cc-point on the edge
    cc_tmp.type = EDGE_CONE;
    return i.update_ifCCinEdgeAndTrue( t, cc_tmp, p1, p2, (true) );
}

std::string ConeCutter::str() const {
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, ConeCutter c) {
  stream << "ConeCutter (d=" << c.diameter << ", angle=" << c.angle << ", L=" << c.length << ")";
  return stream;
}

} // end namespace
// end file conecutter.cpp