// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/util.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <glog/logging.h>

#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/util/random.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

// For an E or F that is defined such that y^t * E * x = 0
double SquaredSampsonDistance(const Matrix3d& F,
                              const Vector2d& x,
                              const Vector2d& y) {
  const Vector3d epiline_x = F * x.homogeneous();
  const double numerator_sqrt = y.homogeneous().dot(epiline_x);
  const Vector4d denominator(y.homogeneous().dot(F.col(0)),
                             y.homogeneous().dot(F.col(1)),
                             epiline_x[0],
                             epiline_x[1]);

  // Finally, return the complete Sampson distance.
  return numerator_sqrt * numerator_sqrt / denominator.squaredNorm();
}

Eigen::Matrix3d CrossProductMatrix(const Vector3d& cross_vec) {
  Matrix3d cross;
  cross << 0.0, -cross_vec.z(), cross_vec.y(), cross_vec.z(), 0.0,
      -cross_vec.x(), -cross_vec.y(), cross_vec.x(), 0.0;
  return cross;
}

// Computes the normalization matrix transformation that centers image points
// around the origin with an average distance of sqrt(2) to the centroid.
// Returns the transformation matrix and the transformed points. This assumes
// that no points are at infinity.
bool NormalizeImagePoints(const std::vector<Vector2d>& image_points,
                          std::vector<Vector2d>* normalized_image_points,
                          Matrix3d* normalization_matrix) {
  Eigen::Map<const Matrix<double, 2, Eigen::Dynamic> > image_points_mat(
      image_points[0].data(), 2, image_points.size());

  // Allocate the output vector and map an Eigen object to the underlying data
  // for efficient calculations.
  normalized_image_points->resize(image_points.size());
  Eigen::Map<Matrix<double, 2, Eigen::Dynamic> > normalized_image_points_mat(
      (*normalized_image_points)[0].data(), 2, image_points.size());

  // Compute centroid.
  const Vector2d centroid(image_points_mat.rowwise().mean());

  // Calculate average RMS distance to centroid.
  const double rms_mean_dist =
      sqrt((image_points_mat.colwise() - centroid).squaredNorm() /
           image_points.size());

  // Create normalization matrix.
  const double norm_factor = sqrt(2.0) / rms_mean_dist;
  *normalization_matrix << norm_factor, 0, -1.0 * norm_factor * centroid.x(), 0,
      norm_factor, -1.0 * norm_factor * centroid.y(), 0, 0, 1;

  // Normalize image points.
  const Matrix<double, 3, Eigen::Dynamic> normalized_homog_points =
      (*normalization_matrix) * image_points_mat.colwise().homogeneous();
  normalized_image_points_mat = normalized_homog_points.colwise().hnormalized();

  return true;
}

// Projects a 3x3 matrix to the rotation matrix in SO3 space with the closest
// Frobenius norm. For a matrix with an SVD decomposition M = USV, the nearest
// rotation matrix is R = UV'.
Matrix3d ProjectToRotationMatrix(const Matrix3d& matrix) {
  Eigen::JacobiSVD<Matrix3d> svd(matrix,
                                 Eigen::ComputeFullU | Eigen::ComputeFullV);
  Matrix3d rotation_mat = svd.matrixU() * (svd.matrixV().transpose());

  // The above projection will give a matrix with a determinant +1 or -1. Valid
  // rotation matrices have a determinant of +1.
  if (rotation_mat.determinant() < 0) {
    rotation_mat *= -1.0;
  }

  return rotation_mat;
}

}  // namespace theia


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 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: Guido Kanschat, Texas A&M University, 2009
 */

// 前面几个文件已经在前面的例子中讲过了，因此不再做进一步的评论。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.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/fe/fe_values.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/fe/mapping_q1.h>

// 这里定义了不连续的有限元。它们的使用方式与所有其他有限元相同，不过--正如你在以前的教程程序中所看到的--用户与有限元类的交互并不多：它们被传递给 DoFHandler 和 FEValues 对象，就这样了。

#include <deal.II/fe/fe_dgq.h>

// 我们将使用最简单的求解器，称为Richardson迭代，它代表了一个简单的缺陷修正。这与一个块状SSOR预处理器（定义在precondition_block.h中）相结合，该预处理器使用DG离散化产生的系统矩阵的特殊块状结构。

#include <deal.II/lac/solver_richardson.h>
#include <deal.II/lac/precondition_block.h>

// 我们将使用梯度作为细化指标。

#include <deal.II/numerics/derivative_approximation.h>

// 这里是使用MeshWorker框架的新的包含文件。第一个文件包含了 MeshWorker::DoFInfo, 类，它为局部积分器提供了局部与全局自由度之间的映射。在第二个文件中，我们发现一个类型为 MeshWorker::IntegrationInfo, 的对象，它主要是对一组FEValues对象的封装。文件<tt>meshworker/simple.h</tt>包含了将局部集成数据组装成只包含一个矩阵的全局系统的类。最后，我们将需要在所有的网格单元和面中运行循环的文件。

#include <deal.II/meshworker/dof_info.h>
#include <deal.II/meshworker/integration_info.h>
#include <deal.II/meshworker/simple.h>
#include <deal.II/meshworker/loop.h>

// 像所有的程序一样，我们在完成这一部分时要包括所需的C++头文件，并声明我们要使用dealii命名空间中的对象，不加前缀。

#include <iostream>
#include <fstream>

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

// 首先，我们定义一个描述不均匀边界数据的类。由于只使用它的值，我们实现value_list()，但不定义Function的所有其他函数。

  template <int dim>
  class BoundaryValues : public Function<dim>
  {
  public:
    BoundaryValues() = default;
    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int component = 0) const override;
  };

// 考虑到流动方向，单位方块 $[0,1]^2$ 的流入边界为右边界和下边界。我们在x轴上规定了不连续的边界值1和0，在右边界上规定了值0。该函数在流出边界上的值将不会在DG方案中使用。

  template <int dim>
  void BoundaryValues<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int component) const
  {
    (void)component;
    AssertIndexRange(component, 1);
    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.;
      }
  }

// 最后，一个计算并返回风场的函数  $\beta=\beta(\mathbf x)$  。正如在介绍中所解释的，在2D中我们将使用一个围绕原点的旋转场。在3D中，我们只需不设置 $z$ 分量（即为零），而这个函数在目前的实现中不能用于1D。

  template <int dim>
  Tensor<1, dim> beta(const Point<dim> &p)
  {
    Assert(dim >= 2, ExcNotImplemented());

    Tensor<1, dim> wind_field;
    wind_field[0] = -p[1];
    wind_field[1] = p[0];
    wind_field /= wind_field.norm();

    return wind_field;
  }
// @sect3{The AdvectionProblem class}

// 在这个准备工作之后，我们继续进行这个程序的主类，叫做AdvectionProblem。它基本上是  step-6  的主类。我们没有AffineConstraints对象，因为在DG离散中没有悬挂节点约束。

// 主要的区别只出现在集合函数的实现上，因为在这里，我们不仅需要覆盖面上的通量积分，我们还使用MeshWorker接口来简化涉及的循环。

  template <int dim>
  class AdvectionProblem
  {
  public:
    AdvectionProblem();
    void run();

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

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

// 此外，我们想使用程度为1的DG元素（但这只在构造函数中指定）。如果你想使用不同度数的DG方法，整个程序保持不变，只需在构造函数中用所需的多项式度数替换1。

    FE_DGQ<dim>     fe;
    DoFHandler<dim> dof_handler;

// 接下来的四个成员代表要解决的线性系统。  system_matrix and right_hand_side 是由 assemble_system(), the solution 产生的， solve(). The sparsity_pattern 是用来确定 system_matrix 中非零元素的位置。

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> right_hand_side;

// 最后，我们必须提供集合单元、边界和内表面条款的函数。在MeshWorker框架中，所有单元的循环和大部分操作的设置都将在这个类之外完成，所以我们所要提供的只是这三个操作。他们将在中间对象上工作，首先，我们在这里定义了交给本地集成函数的信息对象的别名，以使我们的生活更轻松。

    using DoFInfo  = MeshWorker::DoFInfo<dim>;
    using CellInfo = MeshWorker::IntegrationInfo<dim>;

// 下面的三个函数是在所有单元和面的通用循环中被调用的。它们是进行实际整合的函数。

// 在我们下面的代码中，这些函数并不访问当前类的成员变量，所以我们可以将它们标记为 static ，并简单地将这些函数的指针传递给MeshWorker框架。然而，如果这些函数想要访问成员变量（或者需要额外的参数，而不是下面指定的参数），我们可以使用lambda函数的设施来为MeshWorker框架提供对象，这些对象就像它们拥有所需的参数数量和类型一样，但实际上已经绑定了其他参数。

    static void integrate_cell_term(DoFInfo &dinfo, CellInfo &info);
    static void integrate_boundary_term(DoFInfo &dinfo, CellInfo &info);
    static void integrate_face_term(DoFInfo & dinfo1,
                                    DoFInfo & dinfo2,
                                    CellInfo &info1,
                                    CellInfo &info2);
  };

// 我们从构造函数开始。 fe 的构造器调用中的1是多项式的度数。

  template <int dim>
  AdvectionProblem<dim>::AdvectionProblem()
    : mapping()
    , fe(1)
    , dof_handler(triangulation)
  {}

  template <int dim>
  void AdvectionProblem<dim>::setup_system()
  {

// 在设置通常的有限元数据结构的函数中，我们首先需要分配DoF。

    dof_handler.distribute_dofs(fe);

// 我们从生成稀疏模式开始。为此，我们首先用系统中出现的耦合物填充一个动态稀疏模式（DynamicSparsityPattern）类型的中间对象。在建立模式之后，这个对象被复制到 sparsity_pattern 并可以被丢弃。

// 为了建立DG离散的稀疏模式，我们可以调用类似于 DoFTools::make_sparsity_pattern, 的函数，它被称为 DoFTools::make_flux_sparsity_pattern:
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

// 最后，我们设置了线性系统的所有组成部分的结构。

    system_matrix.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
    right_hand_side.reinit(dof_handler.n_dofs());
  }
// @sect4{The assemble_system function}

// 这里我们看到了与手工组装的主要区别。我们不需要在单元格和面上写循环，而是将这一切交给MeshWorker框架。为了做到这一点，我们只需要定义局部的集成函数，并使用命名空间 MeshWorker::Assembler 中的一个类来构建全局系统。

  template <int dim>
  void AdvectionProblem<dim>::assemble_system()
  {

// 这是一个神奇的对象，它知道关于数据结构和局部集成的一切。 这是在函数 MeshWorker::loop(), 中做工作的对象，它被下面的 MeshWorker::integration_loop() 隐式调用。在我们提供指针的函数完成局部积分后， MeshWorker::Assembler::SystemSimple 对象将这些数据分配到全局稀疏矩阵和右手边的向量。

    MeshWorker::IntegrationInfoBox<dim> info_box;

// 首先，我们在工作者基类中初始化正交公式和更新标志。对于正交，我们采取安全措施，使用QGauss公式，其点数比使用的多项式度数高一个。由于单元格、边界和内部面的正交率可以独立选择，我们必须把这个值交给三次。

    const unsigned int n_gauss_points = dof_handler.get_fe().degree + 1;
    info_box.initialize_gauss_quadrature(n_gauss_points,
                                         n_gauss_points,
                                         n_gauss_points);

// 这些是我们整合系统时需要的数值类型。它们被添加到单元格、边界和内部面以及内部邻居面所使用的标志中，这是由四个 @p true 值强制执行的。

    info_box.initialize_update_flags();
    UpdateFlags update_flags =
      update_quadrature_points | update_values | update_gradients;
    info_box.add_update_flags(update_flags, true, true, true, true);

// 在准备好<tt>info_box</tt>中的所有数据后，我们初始化其中的FEValues对象。

    info_box.initialize(fe, mapping);

// 到目前为止创建的对象帮助我们在每个单元和面进行局部积分。现在，我们需要一个对象来接收整合后的（本地）数据，并将它们转发给装配程序。

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

// 现在，我们必须创建装配器对象，并告诉它将本地数据放在哪里。这些将是我们的系统矩阵和右手边的数据。

    MeshWorker::Assembler::SystemSimple<SparseMatrix<double>, Vector<double>>
      assembler;
    assembler.initialize(system_matrix, right_hand_side);

// 最后，在所有活动单元上进行积分循环（由第一个参数决定，它是一个活动迭代器）。

// 正如在类声明中声明局部积分函数时的讨论中所指出的，装配积分器类所期望的参数实际上不是函数指针。相反，它们是可以像函数一样被调用的对象，有一定数量的参数。因此，我们也可以在这里传递具有适当的operator()实现的对象，或者如果本地集成器是，例如，非静态成员函数，则可以传递lambda函数。

    MeshWorker::loop<dim,
                     dim,
                     MeshWorker::DoFInfo<dim>,
                     MeshWorker::IntegrationInfoBox<dim>>(
      dof_handler.begin_active(),
      dof_handler.end(),
      dof_info,
      info_box,
      &AdvectionProblem<dim>::integrate_cell_term,
      &AdvectionProblem<dim>::integrate_boundary_term,
      &AdvectionProblem<dim>::integrate_face_term,
      assembler);
  }
// @sect4{The local integrators}

// 这些是给上面调用的 MeshWorker::integration_loop() 的函数。它们计算单元格和面中对系统矩阵和右手边的局部贡献。

  template <int dim>
  void AdvectionProblem<dim>::integrate_cell_term(DoFInfo & dinfo,
                                                  CellInfo &info)
  {

// 首先，让我们从 @p info. 中检索这里使用的一些对象。注意，这些对象可以处理更复杂的结构，因此这里的访问看起来比看起来更复杂。

    const FEValuesBase<dim> &  fe_values    = info.fe_values();
    FullMatrix<double> &       local_matrix = dinfo.matrix(0).matrix;
    const std::vector<double> &JxW          = fe_values.get_JxW_values();

// 有了这些对象，我们像往常一样继续进行局部积分。首先，我们在正交点上循环，计算当前点的平流矢量。

    for (unsigned int point = 0; point < fe_values.n_quadrature_points; ++point)
      {
        const Tensor<1, dim> beta_at_q_point =
          beta(fe_values.quadrature_point(point));

// 我们求解的是一个同质方程，因此在单元项中没有显示出右手。 剩下的就是对矩阵项的积分。

        for (unsigned int i = 0; i < fe_values.dofs_per_cell; ++i)
          for (unsigned int j = 0; j < fe_values.dofs_per_cell; ++j)
            local_matrix(i, j) += -beta_at_q_point *                //
                                  fe_values.shape_grad(i, point) *  //
                                  fe_values.shape_value(j, point) * //
                                  JxW[point];
      }
  }

// 现在对边界条款也是如此。注意，现在我们使用FEValuesBase，即FEFaceValues和FESubfaceValues的基类，以便获得法向量。

  template <int dim>
  void AdvectionProblem<dim>::integrate_boundary_term(DoFInfo & dinfo,
                                                      CellInfo &info)
  {
    const FEValuesBase<dim> &fe_face_values = info.fe_values();
    FullMatrix<double> &     local_matrix   = dinfo.matrix(0).matrix;
    Vector<double> &         local_vector   = dinfo.vector(0).block(0);

    const std::vector<double> &        JxW = fe_face_values.get_JxW_values();
    const std::vector<Tensor<1, dim>> &normals =
      fe_face_values.get_normal_vectors();

    std::vector<double> g(fe_face_values.n_quadrature_points);

    static BoundaryValues<dim> boundary_function;
    boundary_function.value_list(fe_face_values.get_quadrature_points(), g);

    for (unsigned int point = 0; point < fe_face_values.n_quadrature_points;
         ++point)
      {
        const double beta_dot_n =
          beta(fe_face_values.quadrature_point(point)) * normals[point];
        if (beta_dot_n > 0)
          for (unsigned int i = 0; i < fe_face_values.dofs_per_cell; ++i)
            for (unsigned int j = 0; j < fe_face_values.dofs_per_cell; ++j)
              local_matrix(i, j) += beta_dot_n *                           //
                                    fe_face_values.shape_value(j, point) * //
                                    fe_face_values.shape_value(i, point) * //
                                    JxW[point];
        else
          for (unsigned int i = 0; i < fe_face_values.dofs_per_cell; ++i)
            local_vector(i) += -beta_dot_n *                          //
                               g[point] *                             //
                               fe_face_values.shape_value(i, point) * //
                               JxW[point];
      }
  }

// 最后是内部面的条款。这里的区别是，我们收到了两个信息对象，相邻面的每个单元都有一个，我们组装了四个矩阵，每个单元一个，两个用于来回耦合。

  template <int dim>
  void AdvectionProblem<dim>::integrate_face_term(DoFInfo & dinfo1,
                                                  DoFInfo & dinfo2,
                                                  CellInfo &info1,
                                                  CellInfo &info2)
  {

// 对于正交点、权重等，我们使用第一个参数的FEValuesBase对象。

    const FEValuesBase<dim> &fe_face_values = info1.fe_values();
    const unsigned int       dofs_per_cell  = fe_face_values.dofs_per_cell;

// 对于额外的形状函数，我们必须询问邻居的FEValuesBase。

    const FEValuesBase<dim> &fe_face_values_neighbor = info2.fe_values();
    const unsigned int       neighbor_dofs_per_cell =
      fe_face_values_neighbor.dofs_per_cell;

// 然后我们得到对四个局部矩阵的引用。字母u和v分别指的是试验和测试函数。%的数字表示由info1和info2提供的单元。按照惯例，每个信息对象中的两个矩阵指的是各自单元上的试验函数。第一个矩阵包含该单元的内部耦合，而第二个矩阵包含单元之间的耦合。

    FullMatrix<double> &u1_v1_matrix = dinfo1.matrix(0, false).matrix;
    FullMatrix<double> &u2_v1_matrix = dinfo1.matrix(0, true).matrix;
    FullMatrix<double> &u1_v2_matrix = dinfo2.matrix(0, true).matrix;
    FullMatrix<double> &u2_v2_matrix = dinfo2.matrix(0, false).matrix;

// 在这里，按照前面的函数，我们会有本地的右手边向量。幸运的是，界面条款只涉及到解决方案，右手边没有收到任何贡献。

    const std::vector<double> &        JxW = fe_face_values.get_JxW_values();
    const std::vector<Tensor<1, dim>> &normals =
      fe_face_values.get_normal_vectors();

    for (unsigned int point = 0; point < fe_face_values.n_quadrature_points;
         ++point)
      {
        const double beta_dot_n =
          beta(fe_face_values.quadrature_point(point)) * normals[point];
        if (beta_dot_n > 0)
          {

// 这个词我们已经看过了。

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                u1_v1_matrix(i, j) += beta_dot_n *                           //
                                      fe_face_values.shape_value(j, point) * //
                                      fe_face_values.shape_value(i, point) * //
                                      JxW[point];

// 我们另外组装术语  $(\beta\cdot n u,\hat v)_{\partial \kappa_+}$  。

            for (unsigned int k = 0; k < neighbor_dofs_per_cell; ++k)
              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                u1_v2_matrix(k, j) +=
                  -beta_dot_n *                                   //
                  fe_face_values.shape_value(j, point) *          //
                  fe_face_values_neighbor.shape_value(k, point) * //
                  JxW[point];
          }
        else
          {

// 这个我们也已经看过了。

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              for (unsigned int l = 0; l < neighbor_dofs_per_cell; ++l)
                u2_v1_matrix(i, l) +=
                  beta_dot_n *                                    //
                  fe_face_values_neighbor.shape_value(l, point) * //
                  fe_face_values.shape_value(i, point) *          //
                  JxW[point];

// 而这是另一个新的。 $(\beta\cdot n \hat u,\hat v)_{\partial \kappa_-}$  :

            for (unsigned int k = 0; k < neighbor_dofs_per_cell; ++k)
              for (unsigned int l = 0; l < neighbor_dofs_per_cell; ++l)
                u2_v2_matrix(k, l) +=
                  -beta_dot_n *                                   //
                  fe_face_values_neighbor.shape_value(l, point) * //
                  fe_face_values_neighbor.shape_value(k, point) * //
                  JxW[point];
          }
      }
  }
// @sect3{All the rest}

// 对于这个简单的问题，我们使用了最简单的求解器，称为Richardson迭代，它代表了简单的缺陷修正。这与一个块状SSOR预处理相结合，该预处理使用DG离散化产生的系统矩阵的特殊块状结构。这些块的大小是每个单元的DoF数量。在这里，我们使用SSOR预处理，因为我们没有根据流场对DoFs进行重新编号。如果在流的下游方向对DoFs进行重新编号，那么块状的Gauss-Seidel预处理（见PreconditionBlockSOR类，放松=1）会做得更好。

  template <int dim>
  void AdvectionProblem<dim>::solve(Vector<double> &solution)
  {
    SolverControl                    solver_control(1000, 1e-12);
    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);
  }

// 我们根据一个非常简单的细化标准来细化网格，即对解的梯度的近似。由于这里我们考虑的是DG(1)方法（即我们使用片状双线性形状函数），我们可以简单地计算每个单元的梯度。但是我们并不希望我们的细化指标只建立在每个单元的梯度上，而是希望同时建立在相邻单元之间的不连续解函数的跳跃上。最简单的方法是通过差分商计算近似梯度，包括考虑中的单元和其相邻的单元。这是由 DerivativeApproximation 类完成的，它计算近似梯度的方式类似于本教程 step-9 中描述的 GradientEstimation 。事实上， DerivativeApproximation 类是在 step-9 的 GradientEstimation 类之后开发的。与  step-9  中的讨论相关，这里我们考虑  $h^{1+d/2}|\nabla_h u_h|$  。此外，我们注意到，我们不考虑近似的二次导数，因为线性平流方程的解一般不在 $H^2$ 中，而只在 $H^1$ 中（或者，更准确地说：在 $H^1_\beta$ 中，即在方向 $\beta$ 中的导数是可平方整除的函数空间）。

  template <int dim>
  void AdvectionProblem<dim>::refine_grid()
  {

//  DerivativeApproximation 类将梯度计算为浮点精度。这已经足够了，因为它们是近似的，只作为细化指标。

    Vector<float> gradient_indicator(triangulation.n_active_cells());

// 现在，近似梯度被计算出来了

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

//并且它们被单元格按比例放大，系数为 $h^{1+d/2}$  。
    unsigned int cell_no = 0;
    for (const auto &cell : dof_handler.active_cell_iterators())
      gradient_indicator(cell_no++) *=
        std::pow(cell->diameter(), 1 + 1.0 * dim / 2);

// 最后它们作为细化指标。

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

    triangulation.execute_coarsening_and_refinement();
  }

// 这个程序的输出包括自适应细化网格的eps文件和gnuplot格式的数值解。

  template <int dim>
  void AdvectionProblem<dim>::output_results(const unsigned int cycle) const
  {

// 首先将网格写成eps格式。

    {
      const std::string filename = "grid-" + std::to_string(cycle) + ".eps";
      deallog << "Writing grid to <" << filename << ">" << std::endl;
      std::ofstream eps_output(filename);

      GridOut grid_out;
      grid_out.write_eps(triangulation, eps_output);
    }

// 然后以gnuplot格式输出解决方案。

    {
      const std::string filename = "sol-" + std::to_string(cycle) + ".gnuplot";
      deallog << "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(solution, "u");

      data_out.build_patches();

      data_out.write_gnuplot(gnuplot_output);
    }
  }

// 下面的 run 函数与前面的例子类似。

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

        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation);

            triangulation.refine_global(3);
          }
        else
          refine_grid();

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

        setup_system();

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

        assemble_system();
        solve(solution);

        output_results(cycle);
      }
  }
} // namespace Step12

// 下面的 main 函数与前面的例子也类似，不需要注释。

int main()
{
  try
    {
      dealii::deallog.depth_console(5);

      Step12::AdvectionProblem<2> dgmethod;
      dgmethod.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;
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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.
// ----------------------------------------------------------------------------

#include <Eigen/Eigenvalues>
#include <queue>
#include <tuple>

#include "open3d/geometry/KDTreeFlann.h"
#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/TetraMesh.h"
#include "open3d/utility/Console.h"
#include "open3d/utility/Eigen.h"

namespace open3d {

namespace {
using namespace geometry;

Eigen::Vector3d ComputeEigenvector0(const Eigen::Matrix3d &A, double eval0) {
    Eigen::Vector3d row0(A(0, 0) - eval0, A(0, 1), A(0, 2));
    Eigen::Vector3d row1(A(0, 1), A(1, 1) - eval0, A(1, 2));
    Eigen::Vector3d row2(A(0, 2), A(1, 2), A(2, 2) - eval0);
    Eigen::Vector3d r0xr1 = row0.cross(row1);
    Eigen::Vector3d r0xr2 = row0.cross(row2);
    Eigen::Vector3d r1xr2 = row1.cross(row2);
    double d0 = r0xr1.dot(r0xr1);
    double d1 = r0xr2.dot(r0xr2);
    double d2 = r1xr2.dot(r1xr2);

    double dmax = d0;
    int imax = 0;
    if (d1 > dmax) {
        dmax = d1;
        imax = 1;
    }
    if (d2 > dmax) {
        imax = 2;
    }

    if (imax == 0) {
        return r0xr1 / std::sqrt(d0);
    } else if (imax == 1) {
        return r0xr2 / std::sqrt(d1);
    } else {
        return r1xr2 / std::sqrt(d2);
    }
}

Eigen::Vector3d ComputeEigenvector1(const Eigen::Matrix3d &A,
                                    const Eigen::Vector3d &evec0,
                                    double eval1) {
    Eigen::Vector3d U, V;
    if (std::abs(evec0(0)) > std::abs(evec0(1))) {
        double inv_length =
                1 / std::sqrt(evec0(0) * evec0(0) + evec0(2) * evec0(2));
        U << -evec0(2) * inv_length, 0, evec0(0) * inv_length;
    } else {
        double inv_length =
                1 / std::sqrt(evec0(1) * evec0(1) + evec0(2) * evec0(2));
        U << 0, evec0(2) * inv_length, -evec0(1) * inv_length;
    }
    V = evec0.cross(U);

    Eigen::Vector3d AU(A(0, 0) * U(0) + A(0, 1) * U(1) + A(0, 2) * U(2),
                       A(0, 1) * U(0) + A(1, 1) * U(1) + A(1, 2) * U(2),
                       A(0, 2) * U(0) + A(1, 2) * U(1) + A(2, 2) * U(2));

    Eigen::Vector3d AV = {A(0, 0) * V(0) + A(0, 1) * V(1) + A(0, 2) * V(2),
                          A(0, 1) * V(0) + A(1, 1) * V(1) + A(1, 2) * V(2),
                          A(0, 2) * V(0) + A(1, 2) * V(1) + A(2, 2) * V(2)};

    double m00 = U(0) * AU(0) + U(1) * AU(1) + U(2) * AU(2) - eval1;
    double m01 = U(0) * AV(0) + U(1) * AV(1) + U(2) * AV(2);
    double m11 = V(0) * AV(0) + V(1) * AV(1) + V(2) * AV(2) - eval1;

    double absM00 = std::abs(m00);
    double absM01 = std::abs(m01);
    double absM11 = std::abs(m11);
    double max_abs_comp;
    if (absM00 >= absM11) {
        max_abs_comp = std::max(absM00, absM01);
        if (max_abs_comp > 0) {
            if (absM00 >= absM01) {
                m01 /= m00;
                m00 = 1 / std::sqrt(1 + m01 * m01);
                m01 *= m00;
            } else {
                m00 /= m01;
                m01 = 1 / std::sqrt(1 + m00 * m00);
                m00 *= m01;
            }
            return m01 * U - m00 * V;
        } else {
            return U;
        }
    } else {
        max_abs_comp = std::max(absM11, absM01);
        if (max_abs_comp > 0) {
            if (absM11 >= absM01) {
                m01 /= m11;
                m11 = 1 / std::sqrt(1 + m01 * m01);
                m01 *= m11;
            } else {
                m11 /= m01;
                m01 = 1 / std::sqrt(1 + m11 * m11);
                m11 *= m01;
            }
            return m11 * U - m01 * V;
        } else {
            return U;
        }
    }
}

Eigen::Vector3d FastEigen3x3(Eigen::Matrix3d &A) {
    // Previous version based on:
    // https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3.C3.973_matrices
    // Current version based on
    // https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
    // which handles edge cases like points on a plane

    double max_coeff = A.maxCoeff();
    if (max_coeff == 0) {
        return Eigen::Vector3d::Zero();
    }
    A /= max_coeff;

    double norm = A(0, 1) * A(0, 1) + A(0, 2) * A(0, 2) + A(1, 2) * A(1, 2);
    if (norm > 0) {
        Eigen::Vector3d eval;
        Eigen::Vector3d evec0;
        Eigen::Vector3d evec1;
        Eigen::Vector3d evec2;

        double q = (A(0, 0) + A(1, 1) + A(2, 2)) / 3;

        double b00 = A(0, 0) - q;
        double b11 = A(1, 1) - q;
        double b22 = A(2, 2) - q;

        double p =
                std::sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2) / 6);

        double c00 = b11 * b22 - A(1, 2) * A(1, 2);
        double c01 = A(0, 1) * b22 - A(1, 2) * A(0, 2);
        double c02 = A(0, 1) * A(1, 2) - b11 * A(0, 2);
        double det = (b00 * c00 - A(0, 1) * c01 + A(0, 2) * c02) / (p * p * p);

        double half_det = det * 0.5;
        half_det = std::min(std::max(half_det, -1.0), 1.0);

        double angle = std::acos(half_det) / (double)3;
        double const two_thirds_pi = 2.09439510239319549;
        double beta2 = std::cos(angle) * 2;
        double beta0 = std::cos(angle + two_thirds_pi) * 2;
        double beta1 = -(beta0 + beta2);

        eval(0) = q + p * beta0;
        eval(1) = q + p * beta1;
        eval(2) = q + p * beta2;

        if (half_det >= 0) {
            evec2 = ComputeEigenvector0(A, eval(2));
            if (eval(2) < eval(0) && eval(2) < eval(1)) {
                A *= max_coeff;
                return evec2;
            }
            evec1 = ComputeEigenvector1(A, evec2, eval(1));
            A *= max_coeff;
            if (eval(1) < eval(0) && eval(1) < eval(2)) {
                return evec1;
            }
            evec0 = evec1.cross(evec2);
            return evec0;
        } else {
            evec0 = ComputeEigenvector0(A, eval(0));
            if (eval(0) < eval(1) && eval(0) < eval(2)) {
                A *= max_coeff;
                return evec0;
            }
            evec1 = ComputeEigenvector1(A, evec0, eval(1));
            A *= max_coeff;
            if (eval(1) < eval(0) && eval(1) < eval(2)) {
                return evec1;
            }
            evec2 = evec0.cross(evec1);
            return evec2;
        }
    } else {
        A *= max_coeff;
        if (A(0, 0) < A(1, 1) && A(0, 0) < A(2, 2)) {
            return Eigen::Vector3d(1, 0, 0);
        } else if (A(1, 1) < A(0, 0) && A(1, 1) < A(2, 2)) {
            return Eigen::Vector3d(0, 1, 0);
        } else {
            return Eigen::Vector3d(0, 0, 1);
        }
    }
}

Eigen::Vector3d ComputeNormal(const PointCloud &cloud,
                              const std::vector<int> &indices,
                              bool fast_normal_computation) {
    if (indices.size() == 0) {
        return Eigen::Vector3d::Zero();
    }
    Eigen::Matrix3d covariance =
            utility::ComputeCovariance(cloud.points_, indices);

    if (fast_normal_computation) {
        return FastEigen3x3(covariance);
    } else {
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
        solver.compute(covariance, Eigen::ComputeEigenvectors);
        return solver.eigenvectors().col(0);
    }
}

// Disjoint set data structure to find cycles in graphs
class DisjointSet {
public:
    DisjointSet(size_t size) : parent_(size), size_(size) {
        for (size_t idx = 0; idx < size; idx++) {
            parent_[idx] = idx;
            size_[idx] = 0;
        }
    }

    // find representative element for given x
    // using path compression
    size_t Find(size_t x) {
        if (x != parent_[x]) {
            parent_[x] = Find(parent_[x]);
        }
        return parent_[x];
    }

    // combine two sets using size of sets
    void Union(size_t x, size_t y) {
        x = Find(x);
        y = Find(y);
        if (x != y) {
            if (size_[x] < size_[y]) {
                size_[y] += size_[x];
                parent_[x] = y;
            } else {
                size_[x] += size_[y];
                parent_[y] = x;
            }
        }
    }

private:
    std::vector<size_t> parent_;
    std::vector<size_t> size_;
};

struct WeightedEdge {
    WeightedEdge(size_t v0, size_t v1, double weight)
        : v0_(v0), v1_(v1), weight_(weight) {}
    size_t v0_;
    size_t v1_;
    double weight_;
};

// Minimum Spanning Tree algorithm (Kruskal's algorithm)
std::vector<WeightedEdge> Kruskal(std::vector<WeightedEdge> &edges,
                                  size_t n_vertices) {
    std::sort(edges.begin(), edges.end(),
              [](WeightedEdge &e0, WeightedEdge &e1) {
                  return e0.weight_ < e1.weight_;
              });
    DisjointSet disjoint_set(n_vertices);
    std::vector<WeightedEdge> mst;
    for (size_t eidx = 0; eidx < edges.size(); ++eidx) {
        size_t set0 = disjoint_set.Find(edges[eidx].v0_);
        size_t set1 = disjoint_set.Find(edges[eidx].v1_);
        if (set0 != set1) {
            mst.push_back(edges[eidx]);
            disjoint_set.Union(set0, set1);
        }
    }
    return mst;
}

}  // unnamed namespace

namespace geometry {

void PointCloud::EstimateNormals(
        const KDTreeSearchParam &search_param /* = KDTreeSearchParamKNN()*/,
        bool fast_normal_computation /* = true */) {
    bool has_normal = HasNormals();
    if (!has_normal) {
        normals_.resize(points_.size());
    }
    KDTreeFlann kdtree;
    kdtree.SetGeometry(*this);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)points_.size(); i++) {
        std::vector<int> indices;
        std::vector<double> distance2;
        Eigen::Vector3d normal;
        if (kdtree.Search(points_[i], search_param, indices, distance2) >= 3) {
            normal = ComputeNormal(*this, indices, fast_normal_computation);
            if (normal.norm() == 0.0) {
                if (has_normal) {
                    normal = normals_[i];
                } else {
                    normal = Eigen::Vector3d(0.0, 0.0, 1.0);
                }
            }
            if (has_normal && normal.dot(normals_[i]) < 0.0) {
                normal *= -1.0;
            }
            normals_[i] = normal;
        } else {
            normals_[i] = Eigen::Vector3d(0.0, 0.0, 1.0);
        }
    }
}

void PointCloud::OrientNormalsToAlignWithDirection(
        const Eigen::Vector3d &orientation_reference
        /* = Eigen::Vector3d(0.0, 0.0, 1.0)*/) {
    if (!HasNormals()) {
        utility::LogError(
                "[OrientNormalsToAlignWithDirection] No normals in the "
                "PointCloud. Call EstimateNormals() first.");
    }
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)points_.size(); i++) {
        auto &normal = normals_[i];
        if (normal.norm() == 0.0) {
            normal = orientation_reference;
        } else if (normal.dot(orientation_reference) < 0.0) {
            normal *= -1.0;
        }
    }
}

void PointCloud::OrientNormalsTowardsCameraLocation(
        const Eigen::Vector3d &camera_location /* = Eigen::Vector3d::Zero()*/) {
    if (!HasNormals()) {
        utility::LogError(
                "[OrientNormalsTowardsCameraLocation] No normals in the "
                "PointCloud. Call EstimateNormals() first.");
    }
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)points_.size(); i++) {
        Eigen::Vector3d orientation_reference = camera_location - points_[i];
        auto &normal = normals_[i];
        if (normal.norm() == 0.0) {
            normal = orientation_reference;
            if (normal.norm() == 0.0) {
                normal = Eigen::Vector3d(0.0, 0.0, 1.0);
            } else {
                normal.normalize();
            }
        } else if (normal.dot(orientation_reference) < 0.0) {
            normal *= -1.0;
        }
    }
}

void PointCloud::OrientNormalsConsistentTangentPlane(size_t k) {
    if (!HasNormals()) {
        utility::LogError(
                "[OrientNormalsConsistentTangentPlane] No normals in the "
                "PointCloud. Call EstimateNormals() first.");
    }

    // Create Riemannian graph (Euclidian MST + kNN)
    // Euclidian MST is subgraph of Delaunay triangulation
    std::shared_ptr<TetraMesh> delaunay_mesh;
    std::vector<size_t> pt_map;
    std::tie(delaunay_mesh, pt_map) = TetraMesh::CreateFromPointCloud(*this);
    std::vector<WeightedEdge> delaunay_graph;
    std::unordered_set<size_t> graph_edges;
    auto EdgeIndex = [&](size_t v0, size_t v1) -> size_t {
        return std::min(v0, v1) * points_.size() + std::max(v0, v1);
    };
    auto AddEdgeToDelaunayGraph = [&](size_t v0, size_t v1) {
        v0 = pt_map[v0];
        v1 = pt_map[v1];
        size_t edge = EdgeIndex(v0, v1);
        if (graph_edges.count(edge) == 0) {
            double dist = (points_[v0] - points_[v1]).squaredNorm();
            delaunay_graph.push_back(WeightedEdge(v0, v1, dist));
            graph_edges.insert(edge);
        }
    };
    for (const Eigen::Vector4i &tetra : delaunay_mesh->tetras_) {
        AddEdgeToDelaunayGraph(tetra[0], tetra[1]);
        AddEdgeToDelaunayGraph(tetra[0], tetra[2]);
        AddEdgeToDelaunayGraph(tetra[0], tetra[3]);
        AddEdgeToDelaunayGraph(tetra[1], tetra[2]);
        AddEdgeToDelaunayGraph(tetra[1], tetra[3]);
        AddEdgeToDelaunayGraph(tetra[2], tetra[3]);
    }

    std::vector<WeightedEdge> mst = Kruskal(delaunay_graph, points_.size());

    auto NormalWeight = [&](size_t v0, size_t v1) -> double {
        return 1.0 - std::abs(normals_[v0].dot(normals_[v1]));
    };
    for (auto &edge : mst) {
        edge.weight_ = NormalWeight(edge.v0_, edge.v1_);
    }

    // Add k nearest neighbors to Riemannian graph
    KDTreeFlann kdtree(*this);
    for (size_t v0 = 0; v0 < points_.size(); ++v0) {
        std::vector<int> neighbors;
        std::vector<double> dists2;
        kdtree.SearchKNN(points_[v0], int(k), neighbors, dists2);
        for (size_t vidx1 = 0; vidx1 < neighbors.size(); ++vidx1) {
            size_t v1 = size_t(neighbors[vidx1]);
            if (v0 == v1) {
                continue;
            }
            size_t edge = EdgeIndex(v0, v1);
            if (graph_edges.count(edge) == 0) {
                double weight = NormalWeight(v0, v1);
                mst.push_back(WeightedEdge(v0, v1, weight));
                graph_edges.insert(edge);
            }
        }
    }

    // extract MST from Riemannian graph
    mst = Kruskal(mst, points_.size());

    // convert list of edges to graph
    std::vector<std::unordered_set<size_t>> mst_graph(points_.size());
    for (const auto &edge : mst) {
        size_t v0 = edge.v0_;
        size_t v1 = edge.v1_;
        mst_graph[v0].insert(v1);
        mst_graph[v1].insert(v0);
    }

    // find start node for tree traversal
    // init with node that maximizes z
    double max_z = std::numeric_limits<double>::lowest();
    size_t v0;
    for (size_t vidx = 0; vidx < points_.size(); ++vidx) {
        const Eigen::Vector3d &v = points_[vidx];
        if (v(2) > max_z) {
            max_z = v(2);
            v0 = vidx;
        }
    }

    // traverse MST and orient normals consistently
    std::queue<size_t> traversal_queue;
    std::vector<bool> visited(points_.size(), false);
    traversal_queue.push(v0);
    auto TestAndOrientNormal = [&](const Eigen::Vector3d &n0,
                                   Eigen::Vector3d &n1) {
        if (n0.dot(n1) < 0) {
            n1 *= -1;
        }
    };
    TestAndOrientNormal(Eigen::Vector3d(0, 0, 1), normals_[v0]);
    while (!traversal_queue.empty()) {
        v0 = traversal_queue.front();
        traversal_queue.pop();
        visited[v0] = true;
        for (size_t v1 : mst_graph[v0]) {
            if (!visited[v1]) {
                traversal_queue.push(v1);
                TestAndOrientNormal(normals_[v0], normals_[v1]);
            }
        }
    }
}

}  // namespace geometry
}  // namespace open3d


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_POLYNOMIAL_HPP
#define BOOST_MATH_TOOLS_POLYNOMIAL_HPP

#include <boost/assert.hpp>
#include <boost/math/tools/rational.hpp>
#include <boost/math/tools/real_cast.hpp>

#include <vector>

namespace boost{ namespace math{ namespace tools{

template <class T>
class polynomial
{
public:
   // typedefs:
   typedef typename std::vector<T>::value_type value_type;
   typedef typename std::vector<T>::size_type size_type;

   // construct:
   polynomial(){}
   template <class U>
   polynomial(const U* data, unsigned order)
      : m_data(data, data + order + 1)
   {
   }
   template <class U>
   polynomial(const U& point)
   {
      m_data.push_back(point);
   }

   // copy:
   polynomial(const polynomial& p)
      : m_data(p.m_data) { }

   template <class U>
   polynomial(const polynomial<U>& p)
   {
      for(unsigned i = 0; i < p.size(); ++i)
      {
         m_data.push_back(boost::math::tools::real_cast<T>(p[i]));
      }
   }

   // access:
   size_type size()const { return m_data.size(); }
   size_type degree()const { return m_data.size() - 1; }
   value_type& operator[](size_type i)
   {
      return m_data[i];
   }
   const value_type& operator[](size_type i)const
   {
      return m_data[i];
   }
   T evaluate(T z)const
   {
      return boost::math::tools::evaluate_polynomial(&m_data[0], z, m_data.size());;
   }

   // operators:
   template <class U>
   polynomial& operator +=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(value);
      else
      {
         m_data[0] += value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator -=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(-value);
      else
      {
         m_data[0] -= value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator *=(const U& value)
   {
      for(size_type i = 0; i < m_data.size(); ++i)
         m_data[i] *= value;
      return *this;
   }
   template <class U>
   polynomial& operator +=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] += value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator -=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] -= value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(-value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator *=(const polynomial<U>& value)
   {
      // TODO: FIXME: use O(N log(N)) algorithm!!!
      BOOST_ASSERT(value.size());
      polynomial base(*this);
      *this *= value[0];
      for(size_type i = 1; i < value.size(); ++i)
      {
         polynomial t(base);
         t *= value[i];
         size_type s = size() - i;
         for(size_type j = 0; j < s; ++j)
         {
            m_data[i+j] += t[j];
         }
         for(size_type j = s; j < t.size(); ++j)
            m_data.push_back(t[j]);
      }
      return *this;
   }

private:
   std::vector<T> m_data;
};

template <class T>
inline polynomial<T> operator + (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result += b;
   return result;
}

template <class T>
inline polynomial<T> operator - (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class T>
inline polynomial<T> operator * (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result *= b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator + (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result += b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator - (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator * (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result *= b;
   return result;
}

template <class U, class T>
inline polynomial<T> operator + (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(b);
   result += a;
   return result;
}

template <class U, class T>
inline polynomial<T> operator - (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class U, class T>
inline polynomial<T> operator * (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(b);
   result *= a;
   return result;
}

template <class charT, class traits, class T>
inline std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& os, const polynomial<T>& poly)
{
   os << "{ ";
   for(unsigned i = 0; i < poly.size(); ++i)
   {
      if(i) os << ", ";
      os << poly[i];
   }
   os << " }";
   return os;
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_POLYNOMIAL_HPP


#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <Eigen/Dense>
#include <cmath>

namespace py = pybind11;

void line(double fx1, double fy1, double fx2, double fy2, const Eigen::MatrixXd& global_map,
          Eigen::MatrixXd& op_map) {
  int x1 = static_cast<int>(round(fx1));
  int y1 = static_cast<int>(round(fy1));
  int x2 = static_cast<int>(round(fx2));
  int y2 = static_cast<int>(round(fy2));

  int dx = abs(x2 - x1);
  int dy = abs(y2 - y1);
  int x = x1, y = y1;
  int error = dx - dy;
  int x_inc = x2 > x1 ? 1 : -1;
  int y_inc = y2 > y1 ? 1 : -1;
  dx *= 2;
  dy *= 2;

  int coll_flag = 0;
  int coll_size = 10;

  int rows = global_map.rows();
  int cols = global_map.cols();

  while (x >= 0 && x < cols && y >= 0 && y < rows) {
    if (x == x2 && y == y2) break;

    int k = global_map(y, x);
    if (k != 1 && coll_flag > 0) break;
    if (k == 1 && coll_flag < coll_size) coll_flag += 1;

    op_map(y, x) = k;

    if (k == 1 && coll_flag == coll_size) break;

    if (error > 0) {
      x += x_inc;
      error -= dy;
    } else {
      y += y_inc;
      error += dx;
    }
  }
}

Eigen::MatrixXd inverse_sensor_model(int x0, int y0, int sensor_range,
                            Eigen::MatrixXd op_map, Eigen::MatrixXd global_map) {
  Eigen::MatrixXd op_map_mod(op_map);
  double sensor_angle_inc = 0.5 / 180.0 * M_PI;
  for (double angle = 0.0; angle < M_PI * 2.0; angle += sensor_angle_inc) {
    double x1 = double(x0) + double(sensor_range) * cos(angle);
    double y1 = double (y0) + double(sensor_range) * sin(angle);
    line(x0, y0, x1, y1, global_map, op_map_mod);
  }
  return op_map_mod;
}

PYBIND11_PLUGIN(inverse_sensor_model) {
  py::module m("inverse_sensor_model", "inverse_sensor_model");
  m.def("inverse_sensor_model", &inverse_sensor_model, "inverse_sensor_model");
  return m.ptr();
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file joint_sensor_iid.hpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <memory>
#include <type_traits>

#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/util/meta.hpp>
#include <fl/distribution/gaussian.hpp>

#include <fl/model/adaptive_model.hpp>
#include <fl/model/sensor/interface/sensor_function.hpp>

namespace fl
{

// Forward declarations
template <typename...Models> class JointSensor;

/**
 * Traits of JointSensor<MultipleOf<Sensor, Count>>
 */
template <
    typename Sensor,
    int Count
>
struct Traits<JointSensor<MultipleOf<Sensor, Count>>>
{
    enum : signed int { ModelCount = Count };

    typedef Sensor LocalSensor;
    typedef typename Sensor::State State;
    typedef typename Sensor::Obsrv::Scalar Scalar;
    typedef typename Sensor::Obsrv LocalObsrv;
    typedef typename Sensor::Noise LocalNoise;

    enum : signed int
    {
        StateDim = SizeOf<State>::Value,
        ObsrvDim = ExpandSizes<SizeOf<LocalObsrv>::Value, Count>::Value,
        NoiseDim = ExpandSizes<SizeOf<LocalNoise>::Value, Count>::Value
    };

    typedef Eigen::Matrix<Scalar, ObsrvDim, 1> Obsrv;
    typedef Eigen::Matrix<Scalar, NoiseDim, 1> Noise;

    typedef SensorFunction<Obsrv, State, Noise> SensorFunctionBase;
};

/**
 * \ingroup sensors
 *
 * \brief JointSensor itself is an observation model which contains
 * internally multiple models all of the \em same type. The joint model can be
 * simply summarized as \f$ h(x, \theta, w) = [ h_{local}(x, \theta_1, w_1),
 * h_{local}(x, \theta_2, w_2), \ldots, h_{local}(x, \theta_n, w_n) ]^T \f$
 *
 * where \f$x\f$ is the state variate, \f$\theta = [ \theta_1, \theta_2, \ldots,
 * \theta_n ]^T\f$ and \f$w = [ w_1, w_2, \ldots, w_n ]^T\f$  are the the
 * parameters and noise terms of each of the \f$n\f$ models.
 *
 * JointSensor implements the SensorInterface and the
 * AdaptiveModel interface. That being said, the JointSensor can be
 * used as a regular observation model or even as an adaptive observation model
 * which provides a set of parameters that can be changed at any time. This
 * implies that all sub-models must implement the the AdaptiveModel
 * interface. However, if the sub-model is not adaptive, JointSensor
 * applies a decorator call the NotAdaptive operator on the sub-model. This
 * operator enables any model to be treated as if it is adaptive without
 * effecting the model behaviour.
 */
template <
    typename LocalSensor,
    int Count
>
class JointSensor<MultipleOf<LocalSensor, Count>>
    : public Traits<
                 JointSensor<MultipleOf<LocalSensor, Count>>
             >::SensorFunctionBase,
      public Descriptor,
      private internal::JointSensorIidType
{
private:
    typedef JointSensor<MultipleOf<LocalSensor,Count>> This;

public:
    enum : signed int { ModelCount = Count };

    typedef LocalSensor LocalModel;
    typedef typename Traits<This>::LocalObsrv LocalObsrv;
    typedef typename Traits<This>::LocalNoise LocalNoise;

    typedef typename Traits<This>::Obsrv Obsrv;
    typedef typename Traits<This>::Noise Noise;
    typedef typename Traits<This>::State State;

public:
    JointSensor(
            const LocalSensor& local_sensor,
            int count = ToDimension<Count>::Value)
        : local_sensor_(local_sensor),
          count_(count)
    {
        assert(count_ > 0);
    }

    template <typename Model>
    JointSensor(const MultipleOf<Model, Count>& mof)
        : local_sensor_(mof.instance),
          count_(mof.count)
    {
        assert(count_ > 0);
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~JointSensor() noexcept { }

    Obsrv observation(const State& state, const Noise& noise) const override
    {
        Obsrv y = Obsrv::Zero(obsrv_dimension(), 1);

        const int obsrv_dim = local_sensor_.obsrv_dimension();
        const int noise_dim = local_sensor_.noise_dimension();

        for (int i = 0; i < count_; ++i)
        {
            local_sensor_.id(i);

            y.middleRows(i * obsrv_dim, obsrv_dim) =
                local_sensor_.observation(
                    state,
                    noise.middleRows(i * noise_dim, noise_dim));
        }

        return y;
    }

    int obsrv_dimension() const override
    {
        return local_sensor_.obsrv_dimension() * count_;
    }

    int noise_dimension() const override
    {
        return local_sensor_.noise_dimension() * count_;
    }

    int state_dimension() const override
    {
        return local_sensor_.state_dimension();
    }

    LocalSensor& local_sensor()
    {
        return local_sensor_;
    }

    const LocalSensor& local_sensor() const
    {
        return local_sensor_;
    }

    virtual std::string name() const
    {
        return "JointSensor<MultipleOf<"
                    + this->list_arguments(local_sensor_.name()) +
               ", Count>>";
    }

    virtual std::string description() const
    {
        return "Joint observation model of multiple local observation models "
                " with non-additive noise.";
    }

    /**
     *
     * \brief Returns the number of local models within this joint model
     */
    virtual int count_local_models() const
    {
        return count_;
    }

protected:
    mutable LocalSensor local_sensor_;
    int count_;
};

///**
// * Traits of JointSensor<MultipleOf<Sensor, Count>>
// */
//template <
//    typename Sensor,
//    int Count
//>
//struct Traits<
//           JointSensor<MultipleOf<Sensor, Count>>
//        >
//    : public Traits<
//                JointSensor<
//                    MultipleOf<
//                        typename ForwardAdaptive<Sensor>::Type, Count
//                    >,
//                    Adaptive<>>>
//{ };

///**
// * \internal
// * \ingroup sensors
// *
// * Forwards an adaptive LocalSensor type to the JointSensor
// * implementation. \sa ForwardAdaptive for more details.
// */
//template <
//    typename Sensor,
//    int Count
//>
//class JointSensor<MultipleOf<Sensor, Count>>
//    : public JointSensor<
//                MultipleOf<typename ForwardAdaptive<Sensor>::Type, Count>,
//                Adaptive<>>
//{
//public:
//    typedef JointSensor<
//                MultipleOf<typename ForwardAdaptive<Sensor>::Type, Count>,
//                Adaptive<>
//            > Base;

//    typedef typename ForwardAdaptive<Sensor>::Type ForwardedType;

//    JointSensor(
//            const Sensor& local_sensor,
//            int count = ToDimension<Count>::Value)
//        : Base(ForwardedType(local_sensor), count)
//    { }

//    JointSensor(const MultipleOf<Sensor, Count>& mof)
//        : Base(mof)
//    { }
//};

}


#include <cstdio>
#include <memory>
#include <iostream>
#include <vector>
using namespace std;
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
using namespace boost::accumulators;

// 2 + (3+4)
struct Expression
{
  virtual double eval() = 0;
  virtual void collect(vector<double>& v) = 0;
};

struct Literal : Expression
{
  double value;

  explicit Literal(const double value)
    : value{value}
  {
  }

  double eval() override
  {
    return value;
  }

  void collect(vector<double>& v) override
  {
    v.push_back(value);
  }
};

struct AdditionExpression : Expression
{
  shared_ptr<Expression> left, right;

  AdditionExpression(const shared_ptr<Expression>& expression, const shared_ptr<Expression>& expression1)
    : left{expression},
      right{expression1}
  {
  }

  double eval() override
  {
    return left->eval() + right->eval();
  }

  void collect(vector<double>& v) override
  {
    left->collect(v);
    right->collect(v);
  }
};

int main__3(int ac, char* av)
{
  AdditionExpression sum{
    make_shared<Literal>(2),
    make_shared<AdditionExpression>(
      make_shared<Literal>(3),
      make_shared<Literal>(4)
      )
  };
  cout << "2+(3+4) = " << sum.eval() << endl;

  vector<double> v;
  sum.collect(v);
  for (auto x : v)
    cout << x << "\t";
  cout << endl;

  vector<double> values{ 1,2,3,4 };
  double s = 0;
  for (auto x : values) s += x;
  cout << "average is " << (s / values.size()) << endl;

  accumulator_set<double, stats<tag::mean>> acc;
  for (auto x : values) acc(x);
  cout << "average is " << mean(acc) << endl;

  getchar();
  return 0;
}


/// MIT License
///
/// Copyright (c) 2017 Bjoern Barz
///
/// 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.

#include <math.h>
#include <limits>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Jacobi>     // required for LLT::rankUpdate()

#define ITML_ERR_A0 -1
#define ITML_ERR_NO_CONSTRAINTS -2
#define ITML_ERR_INVALID_CONSTRAINTS -3
#define ITML_ERR_CHOL -4

/**
* Stores the indices of two similar or dissimilar samples.
*/
typedef struct {
    int i; /**< Index of first sample. */
    int j; /**< Index of second sample. */
} itml_pair;

/**
* Learns a Mahalanobis distance metric `(x-y)^T * A * (x-y)` from given data and constraints using
* Information Theoretic Metric Learning (ITML).
*
* ITML minimizes the differential relative entropy between two multivariate Gaussians under constraints
* on the distance function, which can be formulated into a Bregman optimization problem by minimizing the
* LogDet divergence subject to linear constraints.
* Unlike some other methods, ITML does not rely on an eigenvalue computation or semi-definite programming.
*
* The constraints enforced by ITML have the following form:
*
* - `(x-y)^T * A * (x-y) < th_pos` for two similar samples `x` and `y`
* - `(x-y)^T * A * (x-y) > th_neg` for two dissimilar samples `x` and `y`
*
* In theory, individual thresholds could be specified for all pairs, but this implementation only supports
* constant `th_pos` and `th_neg` at the moment.
*
* Reference:
* Jason V. Davis, Brian Kulis, Prateek Jain, Suvrit Sra, Inderjit S. Dhillon.
* "Information-Theoretic Metric Learning."
* International Conference on Machine Learning (ITML), 2007.
*
* @param[in] n The number of samples.
*
* @param[in] d The number of dimensions of the data.
*
* @param[in] pX Pointer to an n-by-d matrix `X` containing one sample per row, stored in row-major order.
*
* @param[in,out] pA Pointer to a row-major d-by-d matrix `A` which initially contains the prior metric serving
* as a regularizer (usually the identity matrix or inverse covariance). The algorithm will update this matrix
* in-place, so that it will finally contain the learned metric or its Cholesky decomposition, depending on the
* value of `return_metric`.
*
* @param[in] nb_pos Number of similarity constraints.
*
* @param[in] pos Pointer to an array of `nb_pos` similarity constraints, given as pairs of indices of similar
* samples in `X`.
*
* @param[in] nb_neg Number of dissimilarity constraints.
*
* @param[in] neg Pointer to an array of `nb_neg` dissimilarity constraints, given as pairs of indices of
* dissimilar samples in `X`.
*
* @param[in] th_pos Threshold for distances of similar samples. ITML enforces the given pairs of similar samples
* to have a distance less than this threshold.
*
* @param[in] th_neg Threshold for distances of dissimilar samples. ITML enforces the given pairs of dissimilar
* samples to have a distance greater than this threshold.
*
* @param[in] return_metric The algorithm actually learns the Cholesky decomposition `U` of the metric `A` with
* `A = U^T * U`, which can be used to transform the data into a space where the Euclidean distance corresponds to
* the learned metric. This matrix `U` will be stored in the matrix pointed to by `pA`. If, however, the actual
* metric `A` is desired, this parameter can be set to `true` to obtain `A` in the matrix pointed to by `pA`.
*
* @param[in] gamma Controls the trade-off between satisyfing the given constraints and minimizing the divergence
* from the prior metric.
* Higher `gamma` puts more weight on the constraints, while lower `gamma` enforces stronger regularization.
*
* @param[in] max_iter Maximum number of iterations.
*
* @param[in] conv_th Convergence threshold.
*
* @param[in] verbose If set to `true`, information about convergence will be written to `stderr` during learning.
*
* @return On success, returns the number of iterations needed until convergence.
* If this is equal to `max_iter`, the algorithm terminated prematurely without reaching convergence.
* In the case of error, one of the following error codes is returned:
*   - `ITML_ERR_A0` (not used anymore): The given prior metric is not positive-semidefinite.
*   - `ITML_ERR_NO_CONSTRAINTS`: No non-trivial constraints have been given.
*   - `ITML_ERR_INVALID_CONSTRAINTS`: Some of the given indices of similar or dissimilar pairs are out of bounds.
*   - `ITML_ERR_CHOL`: Cholesky decomposition of learned metric failed.
*/
template<typename F>
int itml(int n, int d, const F * pX, F * pA,
         int nb_pos, const itml_pair * pos, int nb_neg, const itml_pair * neg, F th_pos, F th_neg,
         bool return_metric = false, F gamma = 1.0, int max_iter = 1000, F conv_th = 0.001, bool verbose = false)
{
    // General local variables
    int i;
    int num_pos, num_neg, num_constraints; // effective number of positive/negative constraints
    const itml_pair * pair;
    const F eps = std::numeric_limits<F>::epsilon();

    // Wrapper around array pointers
    typedef Eigen::Matrix<F, Eigen::Dynamic, 1> Vector;
    typedef Eigen::Matrix<F, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
    Eigen::Map<const Matrix> X(pX, n, d);
    Eigen::Map<Matrix> A(pA, d, d);
    Eigen::SelfAdjointView<Eigen::Map<Matrix>, Eigen::Upper> As = A.template selfadjointView<Eigen::Upper>();

    // Slice rows from X according to constraints
    Matrix vv(nb_pos + nb_neg, d);
    num_pos = num_neg = num_constraints = 0;
    for (i = 0, pair = pos; i < nb_pos; ++i, ++pair)
    {
        if (pair->i < 0 || pair->i >= n || pair->j < 0 || pair->j >= n)
            return ITML_ERR_INVALID_CONSTRAINTS;
        vv.row(num_constraints) = X.row(pair->i) - X.row(pair->j);
        if (vv.row(num_constraints).squaredNorm() > eps)
        {
            ++num_pos;
            ++num_constraints;
        }
    }
    for (i = 0, pair = neg; i < nb_neg; ++i, ++pair)
    {
        if (pair->i < 0 || pair->i >= n || pair->j < 0 || pair->j >= n)
            return ITML_ERR_INVALID_CONSTRAINTS;
        vv.row(num_constraints) = X.row(pair->i) - X.row(pair->j);
        if (vv.row(num_constraints).squaredNorm() > eps)
        {
            ++num_neg;
            ++num_constraints;
        }
    }
    if (num_constraints == 0)
        return ITML_ERR_NO_CONSTRAINTS;

    // Initialize ITML-specific variables
    int sign;
    F dist, alpha, beta, normsum, conv;
    F gamma_proj = std::isinf(gamma) ? 1 : gamma/(gamma+1);
    Vector Av(d);
    Vector lambda = Vector::Zero(num_constraints);
    Vector lambda_old = Vector::Zero(num_constraints);
    Vector bhat(num_constraints);
    bhat.head(num_pos).setConstant(th_pos);
    bhat.tail(num_neg).setConstant(th_neg);

    // Iterative optimization algorithm
    int it;
    for (it = 0; it < max_iter; ++it)
    {
        // Perform update for all constraints
        for (i = 0; i < num_constraints; ++i)
        {
            sign = (i < num_pos) ? 1 : -1;
            Av.noalias() = As * vv.row(i).transpose();
            dist = vv.row(i).dot(Av);
            alpha = std::min(lambda(i), sign * gamma_proj * (1/dist - 1/bhat(i)));
            lambda(i) -= alpha;
            beta = sign * alpha / (1 - sign * alpha * dist);
            bhat(i) = 1 / ((1 / bhat(i)) + sign * (alpha / gamma));
            As.rankUpdate(Av, beta);
        }

        // Check for convergence
        normsum = lambda.norm() + lambda_old.norm();
        if (normsum < eps)
        {
            conv = std::numeric_limits<F>::infinity();
            break;
        }
        conv = (lambda_old - lambda).cwiseAbs().sum();
        conv /= normsum;
        if (conv < conv_th)
            break;
        lambda_old = lambda;
        if (verbose)
            std::cerr << "itml iter: " << it << ", conv = " << conv << std::endl;
    }

    if (verbose)
    {
        if (it < max_iter)
            std::cerr << "itml converged at iter: " << it << ", conv = " << conv << std::endl;
        else
            std::cerr << "itml did not converge after " << it << " iterations, conv = " << conv << std::endl;
    }

    // Store computed metric or its Cholesky decomposition in pA
    A = As;
    if (!return_metric)
    {
        Eigen::LLT<Matrix, Eigen::Upper> llt(A);
        if (llt.info() != Eigen::Success)
            return ITML_ERR_CHOL;
        A = llt.matrixU().toDenseMatrix();
    }

    return it;
}

extern "C"
{

int itml_float(int n, int d, float * pX, float * pA,
               int nb_pos, const itml_pair * pos, int nb_neg, const itml_pair * neg, float th_pos, float th_neg,
               bool return_metric = false, float gamma = 1.0, int max_iter = 1000, float conv_th = 0.001, bool verbose = false)
{
    return itml(n, d, pX, pA, nb_pos, pos, nb_neg, neg, th_pos, th_neg, return_metric, gamma, max_iter, conv_th, verbose);
}

int itml_double(int n, int d, double * pX, double * pA,
                int nb_pos, const itml_pair * pos, int nb_neg, const itml_pair * neg, double th_pos, double th_neg,
                bool return_metric = false, double gamma = 1.0, int max_iter = 1000, double conv_th = 0.001, bool verbose = false)
{
    return itml(n, d, pX, pA, nb_pos, pos, nb_neg, neg, th_pos, th_neg, return_metric, gamma, max_iter, conv_th, verbose);
}

}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file tutorial.cpp

\brief Basic tutorial using SI units.

\details
Tutorial
Defines a function that computes the work, in joules,
done by exerting a force in newtons over a specified distance
in meters and outputs the result to std::cout.

Also code for computing the complex impedance
using std::complex<double> as the value type.

Output:
@verbatim
//[tutorial_output
F  = 2 N
dx = 2 m
E  = 4 J

V   = (12.5,0) V
I   = (3,4) A
Z   = (1.5,-2) Ohm
I*Z = (12.5,0) V
I*Z == V? true
//]
@endverbatim
*/

//[tutorial_code
#include <complex>
#include <iostream>

#include <boost/typeof/std/complex.hpp>

#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/force.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/electric_potential.hpp>
#include <boost/units/systems/si/current.hpp>
#include <boost/units/systems/si/resistance.hpp>
#include <boost/units/systems/si/io.hpp>

using namespace boost::units;
using namespace boost::units::si;

quantity<energy>
work(const quantity<force>& F, const quantity<length>& dx)
{
    return F * dx; // Defines the relation: work = force * distance.
}

int main()
{
    /// Test calculation of work.
    quantity<force>     F(2.0 * newton); // Define a quantity of force.
    quantity<length>    dx(2.0 * meter); // and a distance,
    quantity<energy>    E(work(F,dx));  // and calculate the work done.

    std::cout << "F  = " << F << std::endl
              << "dx = " << dx << std::endl
              << "E  = " << E << std::endl
              << std::endl;

    /// Test and check complex quantities.
    typedef std::complex<double> complex_type; // double real and imaginary parts.

    // Define some complex electrical quantities.
    quantity<electric_potential, complex_type> v = complex_type(12.5, 0.0) * volts;
    quantity<current, complex_type>            i = complex_type(3.0, 4.0) * amperes;
    quantity<resistance, complex_type>         z = complex_type(1.5, -2.0) * ohms;

    std::cout << "V   = " << v << std::endl
              << "I   = " << i << std::endl
              << "Z   = " << z << std::endl
              // Calculate from Ohm's law voltage = current * resistance.
              << "I * Z = " << i * z << std::endl
              // Check defined V is equal to calculated.
              << "I * Z == V? " << std::boolalpha << (i * z == v) << std::endl
              << std::endl;
    return 0;
}
//]


#include <iostream>
#include <unistd>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/timer.hpp>

/*
  First run: 87ns + 74ns dynamic types
             67ns + 42ns static types (r6809) (CET needs 52ns if run alone???)
             71ns + 46ns static types with unrolled by hand (r6810)
*/

#define STATIC_TYPES

#ifdef STATIC_TYPES
   typedef mtl::dense_vector<double, mtl::parameters<mtl::tag::col_major, mtl::fixed::dimension<3>, true> > vec;
#else
   typedef mtl::dense_vector<double> vec;
#endif

using namespace std;

int main()
{

    vec u(3), v(3), w(3), x(3);

    u= 3., 4, 6; v= 7, 9, 3; w= 7, 2, 4;

    const int rep= 10000000;
    boost::timer time;
    for(int i= 0; i < rep; i++) {
	x= dot(v, u) * w + 4.0 * v + 2 * w;
    }
    std::cout << "Compute time (CET) = " << 1000000000.*time.elapsed() / rep << "ns" << std::endl;

    time.restart();
    for(int i= 0; i < rep; i++) {
	x= dot(v, u) * (w+= 4.0 * v + 2 * w);
    }
    std::cout << "Compute time (RET) = " << 1000000000.*time.elapsed() / rep << "ns" << std::endl;

    return 0;
}


#include "eigentypes.h"
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>

#ifndef RUNDOWN
#define RUNDOWN

template < typename Stepper, typename Model, typename Observer >
struct rundown{
  rundown(Model & state, double start, double end, double error = 1e-10 , double firststep = 0.01){
    using namespace boost::numeric::odeint;

    // define stepper with error control works only with C++11 standard!
    auto cstepper = make_controlled( error , error , Stepper() );

  // check whether input scale is lower then output scale:
  if (end>start)
    integrate_adaptive(cstepper, Model(), state, start, end, abs(firststep), Observer());

  else{ // start integration across thresholds

  // get thresholds from state type
  Eigen::VectorXd scales= state.logthresholds();

  double here = start; // helping variable

  // integrate to next threshold
  for (int i=scales.size()-1; i>=0; i--){
    if (scales[i] > end){
      integrate_adaptive(cstepper, Model(), state, here, scales[i], -abs(firststep), Observer());
      state.integrate_out(i);
      here = scales[i];
    }
    else integrate_adaptive(cstepper, Model(), state, here, end, -abs(firststep), Observer());
  }

  // integrate to end
  if (scales[0] > end)
    integrate_adaptive(cstepper, Model(), state, scales[0], end, -abs(firststep), Observer());
  } // else
  } // contructor
};

// specialisation without observer
template < typename Stepper, typename Model >
struct rundown <Stepper, Model, void>{
  rundown(Model & state, double start, double end, double error = 1e-10 , double firststep = 0.01){
      // define stepper with error control works only with C++11 standard!
    auto cstepper = make_controlled( error , error , Stepper() );
  using namespace boost::numeric::odeint;

  // check whether input scale is lower then output scale:
  if (end>start)
    integrate_adaptive(cstepper, Model(), state, start, end, abs(firststep));

  else{ // start integration across thresholds

  // get thresholds from state type
    Eigen::VectorXd scales= state.logthresholds();

  double here = start; // helping vairable

  // integrate to next threshold
  for (int i=scales.size()-1; i>=0; i--){
    if (scales[i] > end){
      integrate_adaptive(cstepper, Model(), state, here, scales[i], -abs(firststep));
      state.integrate_out(i);
      here = scales[i];
    }
    else integrate_adaptive(cstepper, Model(), state, here, end, -abs(firststep));
  }

  // integrate to end
  if (scales[0] > end)
    integrate_adaptive(cstepper, Model(), state, scales[0], end, -abs(firststep));
  } // else
  } // contructor
};

#endif


// File: element_matrix.cpp

#include <iostream>
#include <vector>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

template <typename Matrix>
void fill(Matrix& m)
{
    // Matrices are not initialized by default
    m= 0.0;

    // Type of m's elements
    typedef typename Collection<Matrix>::value_type value_type;

    // Create inserter for matrix m
    // Existing values are not overwritten but inserted
    mat::inserter<Matrix, update_plus<value_type> > ins(m, 3);

    // Define element matrix (array)
    double m1[2][2]= {{1.0, -.4}, {-0.5, 2.0}};

    // Corresponding indices of the elements
    std::vector<int> v1(2);
    v1[0]= 1; v1[1]= 3;

    // Insert element matrix
    ins << element_array(m1, v1);

    // Insert same array with different indices
    v1[0]= 0; v1[1]= 2;
    ins << element_array(m1, v1);

    // Use element matrix type with dynamic size
    dense2D<double> m2(2, 3);
    m2[0][0]= 1; m2[0][1]= 0.2; m2[0][2]= 0.1;
    m2[1][0]= 2; m2[1][1]= 1.2; m2[1][2]= 1.1;

    // Vector for column indices
    dense_vector<int> v2(3);
    // Indices can be out of order
    v2[0]= 4; v2[1]= 1; v2[2]= 3;

    // Use element_matrix and separate vectors for row and column indices
    ins << element_matrix(m2, v1, v2);
}

int main(int, char**)
{
    // Matrices of different types
    compressed2D<double>              A(5, 5);
    dense2D<double>                   B(5, 5);
    morton_dense<float, morton_mask>  C(5, 5);

    // Fill the matrices generically
    fill(A); fill(B); fill(C);
    std::cout << "A is \n" << with_format(A, 4, 3)
	      << "\nB is \n" << with_format(B, 4, 3)
	      << "\nC is \n" << with_format(C, 4, 3);

    return 0;
}


#include <NTL/xdouble.h>
#include <NTL/RR.h>

NTL_START_IMPL

NTL_CHEAP_THREAD_LOCAL
long xdouble::oprec = 10;

void xdouble::SetOutputPrecision(long p)
{
   if (p < 1) p = 1;

   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("xdouble: output precision too big");

   oprec = p;
}

void xdouble::normalize()
{
   if (x == 0)
      e = 0;
   else if (x > 0) {
      while (x < NTL_XD_HBOUND_INV) { x *= NTL_XD_BOUND; e--; }
      while (x > NTL_XD_HBOUND) { x *= NTL_XD_BOUND_INV; e++; }
   }
   else {
      while (x > -NTL_XD_HBOUND_INV) { x *= NTL_XD_BOUND; e--; }
      while (x < -NTL_XD_HBOUND) { x *= NTL_XD_BOUND_INV; e++; }
   }

   if (e >= NTL_OVFBND)
      ResourceError("xdouble: overflow");

   if (e <= -NTL_OVFBND)
      ResourceError("xdouble: underflow");
}

xdouble to_xdouble(double a)
{
   if (a == 0 || a == 1 || (a > 0 && a >= NTL_XD_HBOUND_INV && a <= NTL_XD_HBOUND)
       || (a < 0 && a <= -NTL_XD_HBOUND_INV && a >= -NTL_XD_HBOUND)) {

      return xdouble(a, 0);

   }

   if (!IsFinite(&a))
      ArithmeticError("double to xdouble conversion: non finite value");

   xdouble z = xdouble(a, 0);
   z.normalize();
   return z;
}

void conv(double& xx, const xdouble& a)
{
   double x;
   long e;

   x = a.x;
   e = a.e;

   while (e > 0) { x *= NTL_XD_BOUND; e--; }
   while (e < 0) { x *= NTL_XD_BOUND_INV; e++; }

   xx = x;
}

xdouble operator+(const xdouble& a, const xdouble& b)
{
   xdouble z;

   if (a.x == 0)
      return b;

   if (b.x == 0)
     return a;

   if (a.e == b.e) {
      z.x = a.x + b.x;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else if (a.e > b.e) {
      if (a.e > b.e+1)
         return a;

      z.x = a.x + b.x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else {
      if (b.e > a.e+1)
         return b;

      z.x = a.x*NTL_XD_BOUND_INV + b.x;
      z.e = b.e;
      z.normalize();
      return z;
   }
}

xdouble operator-(const xdouble& a, const xdouble& b)
{
   xdouble z;

   if (a.x == 0)
      return -b;

   if (b.x == 0)
      return a;

   if (a.e == b.e) {
      z.x = a.x - b.x;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else if (a.e > b.e) {
      if (a.e > b.e+1)
         return a;

      z.x = a.x - b.x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else {
      if (b.e > a.e+1)
         return -b;

      z.x = a.x*NTL_XD_BOUND_INV - b.x;
      z.e = b.e;
      z.normalize();
      return z;
   }
}

xdouble operator-(const xdouble& a)
{
   xdouble z;
   z.x = -a.x;
   z.e = a.e;
   return z;
}

xdouble operator*(const xdouble& a, const xdouble& b)
{
   xdouble z;

   z.e = a.e + b.e;
   z.x = a.x * b.x;
   z.normalize();
   return z;
}

xdouble operator/(const xdouble& a, const xdouble& b)
{
   xdouble z;

   if (b.x == 0) ArithmeticError("xdouble division by 0");

   z.e = a.e - b.e;
   z.x = a.x / b.x;
   z.normalize();
   return z;
}

long compare(const xdouble& a, const xdouble& b)
{
   xdouble z = a - b;

   if (z.x < 0)
      return -1;
   else if (z.x == 0)
      return 0;
   else
      return 1;
}

long sign(const xdouble& z)
{
   if (z.x < 0)
      return -1;
   else if (z.x == 0)
      return 0;
   else
      return 1;
}

xdouble trunc(const xdouble& a)
{
   if (a.x >= 0)
      return floor(a);
   else
      return ceil(a);
}

xdouble floor(const xdouble& aa)
{
   xdouble z;

   xdouble a = aa;
   ForceToMem(&a.x);

   if (a.e == 0) {
      z.x = floor(a.x);
      z.e = 0;
      z.normalize();
      return z;
   }
   else if (a.e > 0) {
      return a;
   }
   else {
      if (a.x < 0)
         return to_xdouble(-1);
      else
         return to_xdouble(0);
   }
}

xdouble ceil(const xdouble& aa)
{
   xdouble z;

   xdouble a = aa;
   ForceToMem(&a.x);

   if (a.e == 0) {
      z.x = ceil(a.x);
      z.e = 0;
      z.normalize();
      return z;
   }
   else if (a.e > 0) {
      return a;
   }
   else {
      if (a.x < 0)
         return to_xdouble(0);
      else
         return to_xdouble(1);
   }
}

xdouble to_xdouble(const ZZ& a)
{
   RRPush push;
   RR::SetPrecision(NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   conv(t, a);

   double x;
   conv(x, t.mantissa());

   xdouble y, z, res;

   conv(y, x);
   power2(z, t.exponent());

   res = y*z;

   return res;
}

void conv(ZZ& x, const xdouble& a)
{
   xdouble b = floor(a);

   RRPush push;
   RR::SetPrecision(NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   conv(t, b);
   conv(x, t);
}

xdouble fabs(const xdouble& a)
{
   xdouble z;

   z.e = a.e;
   z.x = fabs(a.x);
   return z;
}

xdouble sqrt(const xdouble& a)
{
   if (a == 0)
      return to_xdouble(0);

   if (a < 0)
      ArithmeticError("xdouble: sqrt of negative number");

   xdouble t;

   if (a.e & 1) {
      t.e = (a.e - 1)/2;
      t.x = sqrt(a.x * NTL_XD_BOUND);
   }
   else {
      t.e = a.e/2;
      t.x = sqrt(a.x);
   }

   t.normalize();

   return t;
}

void power(xdouble& z, const xdouble& a, const ZZ& e)
{
   xdouble b, res;

   b = a;

   res = 1;
   long n = NumBits(e);
   long i;

   for (i = n-1; i >= 0; i--) {
      res = res * res;
      if (bit(e, i))
         res = res * b;
   }

   if (sign(e) < 0)
      z = 1/res;
   else
      z = res;
}

void power(xdouble& z, const xdouble& a, long e)
{
   NTL_ZZRegister(E);
   E = e;
   power(z, a, E);
}

void power2(xdouble& z, long e)
{
   long hb = NTL_XD_HBOUND_LOG;
   long b = 2*hb;

   long q, r;

   q = e/b;
   r = e%b;

   while (r >= hb) {
      r -= b;
      q++;
   }

   while (r < -hb) {
      r += b;
      q--;
   }

   if (q >= NTL_OVFBND)
      ResourceError("xdouble: overflow");

   if (q <= -NTL_OVFBND)
      ResourceError("xdouble: underflow");

   double x = _ntl_ldexp(1.0, r);

   z.x = x;
   z.e = q;
}

void MulAdd(xdouble& z, const xdouble& a, const xdouble& b, const xdouble& c)
// z = a + b*c
{
   double x;
   long e;

   e = b.e + c.e;
   x = b.x * c.x;

   if (x == 0) {
      z = a;
      return;
   }

   if (a.x == 0) {
      z.e = e;
      z.x = x;
      z.normalize();
      return;
   }

   if (a.e == e) {
      z.x = a.x + x;
      z.e = e;
      z.normalize();
      return;
   }
   else if (a.e > e) {
      if (a.e > e+1) {
         z = a;
         return;
      }

      z.x = a.x + x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return;
   }
   else {
      if (e > a.e+1) {
         z.x = x;
         z.e = e;
         z.normalize();
         return;
      }

      z.x = a.x*NTL_XD_BOUND_INV + x;
      z.e = e;
      z.normalize();
      return;
   }
}

void MulSub(xdouble& z, const xdouble& a, const xdouble& b, const xdouble& c)
// z = a - b*c
{
   double x;
   long e;

   e = b.e + c.e;
   x = b.x * c.x;

   if (x == 0) {
      z = a;
      return;
   }

   if (a.x == 0) {
      z.e = e;
      z.x = -x;
      z.normalize();
      return;
   }

   if (a.e == e) {
      z.x = a.x - x;
      z.e = e;
      z.normalize();
      return;
   }
   else if (a.e > e) {
      if (a.e > e+1) {
         z = a;
         return;
      }

      z.x = a.x - x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return;
   }
   else {
      if (e > a.e+1) {
         z.x = -x;
         z.e = e;
         z.normalize();
         return;
      }

      z.x = a.x*NTL_XD_BOUND_INV - x;
      z.e = e;
      z.normalize();
      return;
   }
}

double log(const xdouble& a)
{
   static const double LogBound = log(NTL_XD_BOUND); // GLOBAL (assumes C++11 thread-safe init)
   if (a.x <= 0) {
      ArithmeticError("log(xdouble): argument must be positive");
   }

   return log(a.x) + a.e*LogBound;
}

xdouble xexp(double x)
{
   const double LogBound = log(NTL_XD_BOUND);

   double y = x/LogBound;
   double iy = floor(y+0.5);

   if (iy >= NTL_OVFBND)
      ResourceError("xdouble: overflow");

   if (iy <= -NTL_OVFBND)
      ResourceError("xdouble: underflow");

   double fy = y - iy;

   xdouble res;
   res.e = long(iy);
   res.x = exp(fy*LogBound);
   res.normalize();
   return res;
}

/**************  input / output routines **************/

void ComputeLn2(RR&);
void ComputeLn10(RR&);

long ComputeMax10Power()
{
   RRPush push;
   RR::SetPrecision(NTL_BITS_PER_LONG);

   RR ln2, ln10;
   ComputeLn2(ln2);
   ComputeLn10(ln10);

   long k = to_long( to_RR(NTL_OVFBND/2) * ln2 / ln10 );
   return k;
}

xdouble PowerOf10(const ZZ& e)
{
   static NTL_CHEAP_THREAD_LOCAL long init = 0;
   static NTL_CHEAP_THREAD_LOCAL long k = 0;

   NTL_TLS_LOCAL(xdouble, v10k);

   if (!init) {
      k = ComputeMax10Power();
      RRPush push;
      RR::SetPrecision(NTL_DOUBLE_PRECISION);
      v10k = to_xdouble(power(to_RR(10), k));
      init = 1;
   }

   ZZ e1;
   long neg;

   if (e < 0) {
      e1 = -e;
      neg = 1;
   }
   else {
      e1 = e;
      neg = 0;
   }

   long r;
   ZZ q;

   r = DivRem(q, e1, k);

   RRPush push;
   RR::SetPrecision(NTL_DOUBLE_PRECISION);
   xdouble x1 = to_xdouble(power(to_RR(10), r));

   xdouble x2 = power(v10k, q);
   xdouble x3 = x1*x2;

   if (neg) x3 = 1/x3;

   return x3;
}

ostream& operator<<(ostream& s, const xdouble& a)
{
   if (a == 0) {
      s << "0";
      return s;
   }

   RRPush push;
   long temp_p = long(log(fabs(log(fabs(a))) + 1.0)/log(2.0)) + 10;
   RR::SetPrecision(temp_p);

   RR ln2, ln10, log_2_10;
   ComputeLn2(ln2);
   ComputeLn10(ln10);
   log_2_10 = ln10/ln2;
   ZZ log_10_a = to_ZZ(
  (to_RR(a.e)*to_RR(2*NTL_XD_HBOUND_LOG) + log(fabs(a.x))/log(2.0))/log_2_10);

   xdouble b;
   long neg;

   if (a < 0) {
      b = -a;
      neg = 1;
   }
   else {
      b = a;
      neg = 0;
   }

   ZZ k = xdouble::OutputPrecision() - log_10_a;

   xdouble c, d;

   c = PowerOf10(to_ZZ(xdouble::OutputPrecision()));
   d = PowerOf10(log_10_a);

   b = b / d;
   b = b * c;

   while (b < c) {
      b = b * 10.0;
      k++;
   }

   while (b >= c) {
      b = b / 10.0;
      k--;
   }

   b = b + 0.5;
   k = -k;

   ZZ B;
   conv(B, b);

   long bp_len = xdouble::OutputPrecision()+10;

   UniqueArray<char> bp_store;
   bp_store.SetLength(bp_len);
   char *bp = bp_store.get();

   long len, i;

   len = 0;
   do {
      if (len >= bp_len) LogicError("xdouble output: buffer overflow");
      bp[len] = IntValToChar(DivRem(B, B, 10));
      len++;
   } while (B > 0);

   for (i = 0; i < len/2; i++) {
      char tmp;
      tmp = bp[i];
      bp[i] = bp[len-1-i];
      bp[len-1-i] = tmp;
   }

   i = len-1;
   while (bp[i] == '0') i--;

   k += (len-1-i);
   len = i+1;

   bp[len] = '\0';

   if (k > 3 || k < -len - 3) {
      // use scientific notation

      if (neg) s << "-";
      s << "0." << bp << "e" << (k + len);
   }
   else {
      long kk = to_long(k);

      if (kk >= 0) {
         if (neg) s << "-";
         s << bp;
         for (i = 0; i < kk; i++)
            s << "0";
      }
      else if (kk <= -len) {
         if (neg) s << "-";
         s << "0.";
         for (i = 0; i < -len-kk; i++)
            s << "0";
         s << bp;
      }
      else {
         if (neg) s << "-";
         for (i = 0; i < len+kk; i++)
            s << bp[i];

         s << ".";

         for (i = len+kk; i < len; i++)
            s << bp[i];
      }
   }

   return s;
}

istream& operator>>(istream& s, xdouble& x)
{
   long c;
   long cval;
   long sign;
   ZZ a, b;

   if (!s) NTL_INPUT_ERROR(s, "bad xdouble input");

   c = s.peek();
   while (IsWhiteSpace(c)) {
      s.get();
      c = s.peek();
   }

   if (c == '-') {
      sign = -1;
      s.get();
      c = s.peek();
   }
   else
      sign = 1;

   long got1 = 0;
   long got_dot = 0;
   long got2 = 0;

   a = 0;
   b = 1;

   cval = CharToIntVal(c);

   if (cval >= 0 && cval <= 9) {
      got1 = 1;

      while (cval >= 0 && cval <= 9) {
         mul(a, a, 10);
         add(a, a, cval);
         s.get();
         c = s.peek();
         cval = CharToIntVal(c);
      }
   }

   if (c == '.') {
      got_dot = 1;

      s.get();
      c = s.peek();
      cval = CharToIntVal(c);

      if (cval >= 0 && cval <= 9) {
         got2 = 1;

         while (cval >= 0 && cval <= 9) {
            mul(a, a, 10);
            add(a, a, cval);
            mul(b, b, 10);
            s.get();
            c = s.peek();
            cval = CharToIntVal(c);
         }
      }
   }

   if (got_dot && !got1 && !got2)  NTL_INPUT_ERROR(s, "bad xdouble input");

   ZZ e;

   long got_e = 0;
   long e_sign;

   if (c == 'e' || c == 'E') {
      got_e = 1;

      s.get();
      c = s.peek();

      if (c == '-') {
         e_sign = -1;
         s.get();
         c = s.peek();
      }
      else if (c == '+') {
         e_sign = 1;
         s.get();
         c = s.peek();
      }
      else
         e_sign = 1;

      cval = CharToIntVal(c);

      if (cval < 0 || cval > 9) NTL_INPUT_ERROR(s, "bad xdouble input");

      e = 0;
      while (cval >= 0 && cval <= 9) {
         mul(e, e, 10);
         add(e, e, cval);
         s.get();
         c = s.peek();
         cval = CharToIntVal(c);
      }
   }

   if (!got1 && !got2 && !got_e) NTL_INPUT_ERROR(s, "bad xdouble input");

   xdouble t1, t2, v;

   if (got1 || got2) {
      conv(t1, a);
      conv(t2, b);
      v = t1/t2;
   }
   else
      v = 1;

   if (sign < 0)
      v = -v;

   if (got_e) {
      if (e_sign < 0) negate(e, e);
      t1 = PowerOf10(e);
      v = v * t1;
   }

   x = v;
   return s;
}

NTL_END_IMPL


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_IMPL_LOGS_F_LOG_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_IMPL_LOGS_F_LOG_HPP_INCLUDED

#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/if_else_zero.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/exponential/functions/scalar/impl/logs/f_kernel.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/log_2hi.hpp>
#include <nt2/include/constants/log_2lo.hpp>
#include <nt2/include/constants/log2_em1.hpp>
#include <nt2/include/constants/log10_ehi.hpp>
#include <nt2/include/constants/log10_elo.hpp>
#include <nt2/include/constants/log10_2hi.hpp>
#include <nt2/include/constants/log10_2lo.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <nt2/include/functions/simd/is_nan.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif
#ifndef BOOST_SIMD_NO_DENORMALS
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/constants/smallestposval.hpp>
#include <nt2/include/constants/twotonmb.hpp>
#include <nt2/include/constants/mlogtwo2nmb.hpp>
#include <nt2/include/constants/mlog2two2nmb.hpp>
#include <nt2/include/constants/mlog10two2nmb.hpp>
#endif

  //////////////////////////////////////////////////////////////////////////////
  // how to compute the various logarithms
  //////////////////////////////////////////////////////////////////////////////
  // The method is mainly taken from the cephes library:
  // first reduce the the data
  // a0 is supposed > 0
  // the input a0 is split into a mantissa and an exponent
  // the mantissa m is between sqrt(0.5) and sqrt(2) and the correspondint exponent is e
  // a0 = m*2^e
  // then the log? calculus is split in two parts (? being nothing: natural logarithm,  2: base 2 logarithm,  10 base ten logarithm)
  // as log?(a) = log?(2^e)+log?(m)
  // 1) computing log?(m)
  //   first put x = m-1 (so -0.29 <  x < 0.414)
  //   write log(m)   = log(1+x)   = x + x*x/2 + x*x*x*g(x)
  //   write log2(m)  = log2(1+x)  = C2*log(x)   C2 =  log(2)  the multiplication have to be taken seriously as C2 is not exact
  //   write log10(m) = log10(1+x) = C10*log(x)  C10=  log(10) the multiplication have to be taken seriously as C10 is not exact
  // then g(x) has to be approximated
  // g is ((log(1+x)/x-1)/x-1/2)/x
  // It is not a good idea to approximate directly log(1+x) instead of g,  because this will lead to bad precision around 1.
  //
  // in this approximation one can choose a best approximation rational function given by remez algorithm.
  // there exist a classical solution which is a polynomial p8 one of degree 8 that gives 0.5ulps everywhere
  // this is what is done in the kernel_t::log impl;
  // Now,  it is possible to choose a rational fraction or a polynomial of lesser degree to approximate g
  // providing faster but less accurate logs.
  // 2) computing log?(2^e)
  // see the explanations relative to each case
  // 3) finalize
  // This is simply treating invalid entries
  // 4) For denormal we use the fact that log(x) =  log?(x*y)-log?(y) and that if y is
  // the constant two2nmb if x is denormal x*y and y are not.
  //////////////////////////////////////////////////////////////////////////////

namespace nt2 { namespace details
{
  //////////////////////////////////////////////////////////////////////////////
  // math log functions
  //////////////////////////////////////////////////////////////////////////////

  template < class A0 >
  struct logarithm< A0, tag::simd_type, float>
  {
    typedef typename meta::as_logical<A0>::type                 lA0;
    typedef typename meta::as_integer<A0, signed>::type    int_type;
    typedef typename meta::scalar_of<A0>::type                  sA0;
    typedef kernel<A0, tag::simd_type, float>              kernel_t;

    static inline A0 log(const A0& a0)
    {
      A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
      A0 t = Zero<A0>();
      lA0 denormal = lt(nt2::abs(z), Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      t = if_else_zero(denormal, Mlogtwo2nmb<A0>());
#endif
      //log(2.0) in double is 6.931471805599453e-01
      //double(0.693359375f)+double(-0.00021219444f)  is  6.931471805600000e-01 at 1.0e-14 of log(2.0)
      // let us call Log_2hi 0.693359375f anf Log_2lo -0.00021219444f
      // We use thi to correct the sum where this could matter a lot
      // log(a0) = fe*Log_2hi+ (0.5f*x*x +(fe*Log_2lo+y))
      // These operations are order dependent: the parentheses do matter
      A0 x, fe, x2, y;
      kernel_t::log(z, fe, x, x2, y);
      y = nt2::fma(fe, Log_2lo<A0>(), y);
      y = nt2::fma(Mhalf<A0>(), x2, y);
#ifdef BOOST_SIMD_NO_DENORMALS
      return finalize(a0, nt2::fma(Log_2hi<A0>(), fe, x+y));
#else
      return finalize(a0, nt2::fma(Log_2hi<A0>(), fe, x+y+t));
#endif
    }

    static inline A0 log2(const A0& a0)
    {
      A0 z =  a0;
#ifndef BOOST_SIMD_NO_DENORMALS
      lA0 denormal = lt(nt2::abs(z), Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      A0 t = if_else_zero(denormal, Mlog2two2nmb<A0>());
#endif
      //here let l2em1 = log2(e)-1, the computation is done as:
      //log2(a0) = ((l2em1*x+(l2em1*(y+x*x/2)))+(y+x*x/2)))+x+fe for best results
      // once again the order is very important.
      A0 x, fe, x2, y;
      kernel_t::log(z, fe, x, x2, y);
      y =  nt2::fma(Mhalf<A0>(),x2, y);
      z = nt2::fma(x,Log2_em1<A0>(),y*Log2_em1<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
      return finalize(a0, ((z+y)+x)+fe);
#else
      return finalize(a0, ((z+y)+x)+fe+t);
#endif
    }

    static inline A0 log10(const A0& a0)
    {
      A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
      lA0 denormal = lt(nt2::abs(z), Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      A0 t = if_else_zero(denormal, Mlog10two2nmb<A0>());
#endif
      // here there are two multiplication:  log of fraction by log10(e) and base 2 exponent by log10(2)
      // and we have to split log10(e) and log10(2) in two parts to get extra precision when needed
      A0 x, fe, x2, y;
      kernel_t::log(z, fe, x, x2, y);
      y = nt2::amul(y, Mhalf<A0>(), x2);
      z = mul(x+y, Log10_elo<A0>());
      z = nt2::amul(z, y, Log10_ehi<A0>());
      z = nt2::amul(z, x, Log10_ehi<A0>());
      z = nt2::amul(z, fe, Log10_2hi<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
      return finalize(a0, nt2::amul(z, fe, Log10_2lo<A0>()));
#else
      return finalize(a0, nt2::amul(z+t, fe, Log10_2lo<A0>()));
#endif
    }
  private:
    static inline A0 finalize(const A0& a0, const A0& y)
    {
      typedef typename meta::as_logical<A0>::type              lA0;
    #ifdef BOOST_SIMD_NO_NANS
      lA0 test = nt2::is_ltz(a0);
    #else
      lA0 test = nt2::logical_or(nt2::is_ltz(a0), nt2::is_nan(a0));
    #endif
      A0 y1 = nt2::if_nan_else(test, y);
    #ifndef BOOST_SIMD_NO_INFINITIES
      y1 = if_else(nt2::is_equal(a0, nt2::Inf<A0>()), a0, y1);
    #endif
      return if_else(is_eqz(a0), nt2::Minf<A0>(), y1);
    }
  };
} }

#endif