#include "LonLatDistance.h"

#include <boost/math/constants/constants.hpp>
#include <macgyver/Exception.h>

namespace SmartMet
{
namespace Plugin
{
namespace TimeSeries
{
/// @brief Earth's quatratic mean radius for WGS-84
static const double EARTH_RADIUS_IN_METERS = 6372797.560856;

double deg_to_rad(const double& degrees)
{
  try
  {
    return degrees * (boost::math::constants::pi<double>() / 180.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

double rad_to_deg(const double& radians)
{
  try
  {
    return radians * (180.0 / boost::math::constants::pi<double>());
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns the (initial) bearing from this point to the supplied point, in degrees
 *   see http://williams.best.vwh.net/avform.htm#Crs
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Initial bearing in degrees from North
 */
double initial_bearing(const std::pair<double, double>& from, const std::pair<double, double>& to)
{
  try
  {
    double lat1 = deg_to_rad(from.second);
    double lat2 = deg_to_rad(to.second);
    double dLon = deg_to_rad(to.first - from.first);

    double y = sin(dLon) * cos(lat2);
    double x = (cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(dLon));
    double brng = atan2(y, x);

    return fmod((rad_to_deg(brng) + 360.0), 360.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns final bearing arriving at supplied destination point from this point; the final bearing
 * will differ from the initial bearing by varying degrees according to distance and latitude
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {Number} Final bearing in degrees from North
 */
double final_bearing(const std::pair<double, double>& from, const std::pair<double, double>& to)
{
  try
  {
    // get initial bearing from supplied point back to this point...
    double lat1 = deg_to_rad(to.second);
    double lat2 = deg_to_rad(from.second);
    double dLon = deg_to_rad(from.first - to.first);

    double y = sin(dLon) * cos(lat2);
    double x = (cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(dLon));
    double brng = atan2(y, x);
    // ... & reverse it by adding 180°
    return fmod((rad_to_deg(brng) + 180.0), 360.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns the midpoint between this point and the supplied point.
 *   see http://mathforum.org/library/drmath/view/51822.html for derivation
 *
 * @param   {LatLon} point: Latitude/longitude of destination point
 * @returns {LatLon} Midpoint between this point and the supplied point
 */
std::pair<double, double> midpoint(const std::pair<double, double>& from,
                                   const std::pair<double, double>& to)
{
  try
  {
    double lat1 = deg_to_rad(from.second);
    double lon1 = deg_to_rad(from.first);
    double lat2 = deg_to_rad(to.second);
    double dLon = deg_to_rad(to.first - from.first);

    double Bx = cos(lat2) * cos(dLon);
    double By = cos(lat2) * sin(dLon);

    double lat3 = atan2(sin(lat1) + sin(lat2), sqrt((cos(lat1) + Bx) * (cos(lat1) + Bx) + By * By));
    double lon3 = lon1 + atan2(By, cos(lat1) + Bx);
    double pi(boost::math::constants::pi<double>());
    lon3 = fmod(lon3 + (3 * pi), 2 * pi) - pi;  // normalise to -180..+180º

    return std::pair<double, double>(rad_to_deg(lon3), rad_to_deg(lat3));
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/**
 * Returns the destination point from this point having travelled the given distance (in km) on the
 * given initial bearing (bearing may vary before destination is reached)
 *
 *   see http://williams.best.vwh.net/avform.htm#LL
 *
 * @param   {Number} brng: Initial bearing in degrees
 * @param   {Number} dist: Distance in km
 * @returns {LatLon} Destination point
 */
std::pair<double, double> destination_point(const std::pair<double, double>& from,
                                            const std::pair<double, double>& to,
                                            const double& distance)
{
  try
  {
    double brng(initial_bearing(from, to));

    double dist = distance /
                  (EARTH_RADIUS_IN_METERS / 1000.0);  // convert dist to angular distance in radians
    brng = deg_to_rad(brng);
    double lat1 = deg_to_rad(from.second);
    double lon1 = deg_to_rad(from.first);

    double lat2 = asin(sin(lat1) * cos(dist) + cos(lat1) * sin(dist) * cos(brng));
    double lon2 =
        lon1 + atan2(sin(brng) * sin(dist) * cos(lat1), cos(dist) - sin(lat1) * sin(lat2));
    double pi(boost::math::constants::pi<double>());
    lon2 = fmod(lon2 + 3 * pi, 2 * pi) - pi;  // normalise to -180..+180º

    return {rad_to_deg(lon2), rad_to_deg(lat2)};
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/** @brief Computes the arc, in radian, between two WGS-84 positions.
 *
 * The result is equal to Distance(from,to)/EARTH_RADIUS_IN_METERS
 *    = 2*asin(sqrt(h(d/EARTH_RADIUS_IN_METERS )))
 *
 * where:

 *
d is the distance in meters between 'from' and 'to' positions.

 *
h is the haversine function: h(x)=sin²(x/2)

 *

 *
 * The haversine formula gives:
 *    h(d/R) = h(from.lat-to.lat)+h(from.lon-to.lon)+cos(from.lat)*cos(to.lat)
 *
 * @sa http://en.wikipedia.org/wiki/Law_of_haversines
 */
double arc_in_radians(const std::pair<double, double>& from, const std::pair<double, double>& to)
{
  try
  {
    double latitudeArc = deg_to_rad(from.second - to.second);
    double longitudeArc = deg_to_rad(from.first - to.first);
    double latitudeH = sin(latitudeArc * 0.5);
    latitudeH *= latitudeH;
    double lontitudeH = sin(longitudeArc * 0.5);
    lontitudeH *= lontitudeH;
    double tmp = cos(deg_to_rad(from.second)) * cos(deg_to_rad(to.second));
    return 2.0 * asin(sqrt(latitudeH + tmp * lontitudeH));
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

/** @brief Computes the distance, in meters, between two WGS-84 positions.
 *
 * The result is equal to EARTH_RADIUS_IN_METERS*ArcInRadians(from,to)
 *
 * @sa ArcInRadians
 */
double distance_in_meters(const std::pair<double, double>& from,
                          const std::pair<double, double>& to)
{
  try
  {
    return EARTH_RADIUS_IN_METERS * arc_in_radians(from, to);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

double distance_in_kilometers(const std::pair<double, double>& from,
                              const std::pair<double, double>& to)
{
  try
  {
    return (distance_in_meters(from, to) / 1000.0);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP, "Operation failed!", nullptr);
  }
}

}  // namespace TimeSeries
}  // namespace Plugin
}  // namespace SmartMet


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/Simplex_tree.h>
#include <gudhi/Points_3D_off_io.h>

#include <boost/variant.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Alpha_shape_3.h>
#include <CGAL/Alpha_shape_vertex_base_3.h>
#include <CGAL/Alpha_shape_cell_base_3.h>
#include <CGAL/iterator.h>

#include <fstream>
#include <cmath>
#include <string>
#include <tuple>  // for tuple<>
#include <map>
#include <utility>  // for pair<>
#include <list>
#include <vector>

// Alpha_shape_3 templates type definitions
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef CGAL::Alpha_shape_vertex_base_3<Kernel> Vb;
typedef CGAL::Alpha_shape_cell_base_3<Kernel> Fb;
typedef CGAL::Triangulation_data_structure_3<Vb, Fb> Tds;
typedef CGAL::Delaunay_triangulation_3<Kernel, Tds> Triangulation_3;
typedef CGAL::Alpha_shape_3<Triangulation_3> Alpha_shape_3;

// From file type definition
typedef Kernel::Point_3 Point;

// filtration with alpha values needed type definition
typedef Alpha_shape_3::FT Alpha_value_type;
typedef CGAL::Object Object;
typedef CGAL::Dispatch_output_iterator<
CGAL::cpp11::tuple<Object, Alpha_value_type>,
CGAL::cpp11::tuple<std::back_insert_iterator< std::vector<Object> >,
    std::back_insert_iterator< std::vector<Alpha_value_type> > > > Dispatch;
typedef Alpha_shape_3::Cell_handle Cell_handle;
typedef Alpha_shape_3::Facet Facet;
typedef Alpha_shape_3::Edge Edge;
typedef std::list<Alpha_shape_3::Vertex_handle> Vertex_list;

// gudhi type definition
typedef Gudhi::Simplex_tree<> Simplex_tree;
typedef Simplex_tree::Vertex_handle Simplex_tree_vertex;
typedef std::map<Alpha_shape_3::Vertex_handle, Simplex_tree_vertex > Alpha_shape_simplex_tree_map;
typedef std::pair<Alpha_shape_3::Vertex_handle, Simplex_tree_vertex> Alpha_shape_simplex_tree_pair;
typedef std::vector< Simplex_tree_vertex > Simplex_tree_vector_vertex;

Vertex_list from(const Cell_handle& ch) {
  Vertex_list the_list;
  for (auto i = 0; i < 4; i++) {
#ifdef DEBUG_TRACES
    std::cout << "from cell[" << i << "]=" << ch->vertex(i)->point() << std::endl;
#endif  // DEBUG_TRACES
    the_list.push_back(ch->vertex(i));
  }
  return the_list;
}

Vertex_list from(const Facet& fct) {
  Vertex_list the_list;
  for (auto i = 0; i < 4; i++) {
    if (fct.second != i) {
#ifdef DEBUG_TRACES
      std::cout << "from facet=[" << i << "]" << fct.first->vertex(i)->point() << std::endl;
#endif  // DEBUG_TRACES
      the_list.push_back(fct.first->vertex(i));
    }
  }
  return the_list;
}

Vertex_list from(const Edge& edg) {
  Vertex_list the_list;
  for (auto i = 0; i < 4; i++) {
    if ((edg.second == i) || (edg.third == i)) {
#ifdef DEBUG_TRACES
      std::cout << "from edge[" << i << "]=" << edg.first->vertex(i)->point() << std::endl;
#endif  // DEBUG_TRACES
      the_list.push_back(edg.first->vertex(i));
    }
  }
  return the_list;
}

Vertex_list from(const Alpha_shape_3::Vertex_handle& vh) {
  Vertex_list the_list;
#ifdef DEBUG_TRACES
  std::cout << "from vertex=" << vh->point() << std::endl;
#endif  // DEBUG_TRACES
  the_list.push_back(vh);
  return the_list;
}

int main(int argc, char * const argv[]) {
  // program args management
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0]
        << " path_to_off_file \n";
    return 0;
  }

  // Read points from file
  std::string offInputFile(argv[1]);
  // Read the OFF file (input file name given as parameter) and triangulate points
  Gudhi::Points_3D_off_reader<Point> off_reader(offInputFile);
  // Check the read operation was correct
  if (!off_reader.is_valid()) {
    std::cerr << "Unable to read file " << argv[1] << std::endl;
    return 0;
  }
  // Retrieve the triangulation
  std::vector<Point> lp = off_reader.get_point_cloud();

  // alpha shape construction from points. CGAL has a strange behavior in REGULARIZED mode.
  Alpha_shape_3 as(lp.begin(), lp.end(), 0, Alpha_shape_3::GENERAL);
#ifdef DEBUG_TRACES
  std::cout << "Alpha shape computed in GENERAL mode" << std::endl;
#endif  // DEBUG_TRACES

  // filtration with alpha values from alpha shape
  std::vector<Object> the_objects;
  std::vector<Alpha_value_type> the_alpha_values;

  Dispatch disp = CGAL::dispatch_output<Object, Alpha_value_type>(std::back_inserter(the_objects),
      std::back_inserter(the_alpha_values));

  as.filtration_with_alpha_values(disp);
#ifdef DEBUG_TRACES
  std::cout << "filtration_with_alpha_values returns : " << the_objects.size() << " objects" << std::endl;
#endif  // DEBUG_TRACES

  Alpha_shape_3::size_type count_vertices = 0;
  Alpha_shape_3::size_type count_edges = 0;
  Alpha_shape_3::size_type count_facets = 0;
  Alpha_shape_3::size_type count_cells = 0;

  // Loop on objects vector
  Vertex_list vertex_list;
  Simplex_tree simplex_tree;
  Alpha_shape_simplex_tree_map map_cgal_simplex_tree;
  std::vector<Alpha_value_type>::iterator the_alpha_value_iterator = the_alpha_values.begin();
  for (auto object_iterator : the_objects) {
    // Retrieve Alpha shape vertex list from object
    if (const Cell_handle * cell = CGAL::object_cast<Cell_handle>(&object_iterator)) {
      vertex_list = from(*cell);
      count_cells++;
    } else if (const Facet * facet = CGAL::object_cast<Facet>(&object_iterator)) {
      vertex_list = from(*facet);
      count_facets++;
    } else if (const Edge * edge = CGAL::object_cast<Edge>(&object_iterator)) {
      vertex_list = from(*edge);
      count_edges++;
    } else if (const Alpha_shape_3::Vertex_handle * vertex =
              CGAL::object_cast<Alpha_shape_3::Vertex_handle>(&object_iterator)) {
      count_vertices++;
      vertex_list = from(*vertex);
    }
    // Construction of the vector of simplex_tree vertex from list of alpha_shapes vertex
    Simplex_tree_vector_vertex the_simplex_tree;
    for (auto the_alpha_shape_vertex : vertex_list) {
      Alpha_shape_simplex_tree_map::iterator the_map_iterator = map_cgal_simplex_tree.find(the_alpha_shape_vertex);
      if (the_map_iterator == map_cgal_simplex_tree.end()) {
        // alpha shape not found
        Simplex_tree_vertex vertex = map_cgal_simplex_tree.size();
#ifdef DEBUG_TRACES
        std::cout << "vertex [" << the_alpha_shape_vertex->point() << "] not found - insert_simplex " << vertex << "\n";
#endif  // DEBUG_TRACES
        the_simplex_tree.push_back(vertex);
        map_cgal_simplex_tree.insert(Alpha_shape_simplex_tree_pair(the_alpha_shape_vertex, vertex));
      } else {
        // alpha shape found
        Simplex_tree_vertex vertex = the_map_iterator->second;
#ifdef DEBUG_TRACES
        std::cout << "vertex [" << the_alpha_shape_vertex->point() << "] found in " << vertex << std::endl;
#endif  // DEBUG_TRACES
        the_simplex_tree.push_back(vertex);
      }
    }
    // Construction of the simplex_tree
#ifdef DEBUG_TRACES
    std::cout << "filtration = " << *the_alpha_value_iterator << std::endl;
#endif  // DEBUG_TRACES
    simplex_tree.insert_simplex(the_simplex_tree, std::sqrt(*the_alpha_value_iterator));
    if (the_alpha_value_iterator != the_alpha_values.end())
      ++the_alpha_value_iterator;
    else
      std::cerr << "This shall not happen" << std::endl;
  }
#ifdef DEBUG_TRACES
  std::cout << "vertices \t\t" << count_vertices << std::endl;
  std::cout << "edges \t\t" << count_edges << std::endl;
  std::cout << "facets \t\t" << count_facets << std::endl;
  std::cout << "cells \t\t" << count_cells << std::endl;

  std::cout << "Information of the Simplex Tree:\n";
  std::cout << "  Number of vertices = " << simplex_tree.num_vertices() << " ";
  std::cout << "  Number of simplices = " << simplex_tree.num_simplices() << std::endl << std::endl;
#endif  // DEBUG_TRACES

#ifdef DEBUG_TRACES
  std::cout << "Iterator on vertices: \n";
  for (auto vertex : simplex_tree.complex_vertex_range()) {
    std::cout << vertex << " ";
  }
#endif  // DEBUG_TRACES

  std::cout << simplex_tree << std::endl;

#ifdef DEBUG_TRACES
  std::cout << std::endl << std::endl << "Iterator on simplices:\n";
  for (auto simplex : simplex_tree.complex_simplex_range()) {
    std::cout << "   ";
    for (auto vertex : simplex_tree.simplex_vertex_range(simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << std::endl;
  }
#endif  // DEBUG_TRACES
#ifdef DEBUG_TRACES
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:\n";
  for (auto f_simplex : simplex_tree.filtration_simplex_range()) {
    std::cout << "   " << "[" << simplex_tree.filtration(f_simplex) << "] ";
    for (auto vertex : simplex_tree.simplex_vertex_range(f_simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << std::endl;
  }
#endif  // DEBUG_TRACES
#ifdef DEBUG_TRACES
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, and their boundary simplices:\n";
  for (auto f_simplex : simplex_tree.filtration_simplex_range()) {
    std::cout << "   " << "[" << simplex_tree.filtration(f_simplex) << "] ";
    for (auto vertex : simplex_tree.simplex_vertex_range(f_simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << std::endl;

    for (auto b_simplex : simplex_tree.boundary_simplex_range(f_simplex)) {
      std::cout << "      " << "[" << simplex_tree.filtration(b_simplex) << "] ";
      for (auto vertex : simplex_tree.simplex_vertex_range(b_simplex)) {
        std::cout << vertex << " ";
      }
      std::cout << std::endl;
    }
  }
#endif  // DEBUG_TRACES

  return 0;
}


#include <CGAL/Simple_cartesian.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>

#include <CGAL/subdivision_method_3.h>
#include <CGAL/Timer.h>

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <fstream>

typedef CGAL::Simple_cartesian<double>          Kernel;
typedef CGAL::Surface_mesh<Kernel::Point_3>     PolygonMesh;

using namespace std;
using namespace CGAL;
namespace params = CGAL::parameters;

int main(int argc, char **argv) {
  if (argc > 4) {
    cerr << "Usage: Sqrt3_subdivision [d] [filename_in] [filename_out] \n";
    cerr << "         d -- the depth of the subdivision (default: 1) \n";
    cerr << "         filename_in -- the input mesh (.off) (default: data/quint_tris.off) \n";
    cerr << "         filename_out -- the output mesh (.off) (default: result.off)" << endl;
    return 1;
  }

  int d = (argc > 1) ? boost::lexical_cast<int>(argv[1]) : 2;
  const std::string in_file = (argc > 2) ? argv[2] : CGAL::data_file_path("meshes/quint_tris.off");
  const char* out_file = (argc > 3) ? argv[3] : "result.off";

  PolygonMesh pmesh;
  std::ifstream in(in_file);
  if(in.fail()) {
    std::cerr << "Could not open input file " << in_file << std::endl;
    return 1;
  }
  in >> pmesh;

  Timer t;
  t.start();
  Subdivision_method_3::Sqrt3_subdivision(pmesh, params::number_of_iterations(d));
  std::cerr << "Done (" << t.time() << " s)" << std::endl;

  std::ofstream out(out_file);
  out << pmesh;

  return 0;
}


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __spatial_explog_hpp__
#define __spatial_explog_hpp__

#include <Eigen/Geometry>

#include "pinocchio/fwd.hpp"
#include "pinocchio/utils/static-if.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/math/taylor-expansion.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/skew.hpp"
#include "pinocchio/spatial/se3.hpp"

namespace pinocchio
{
  /// \brief Exp: so3 -> SO3.
  ///
  /// Return the integral of the input angular velocity during time 1.
  ///
  /// \param[in] v The angular velocity vector.
  ///
  /// \return The rotational matrix associated to the integration of the angular velocity during time 1.
  ///
  template<typename Vector3Like>
  typename Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
  exp3(const Eigen::MatrixBase<Vector3Like> & v)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1);

    typedef typename Vector3Like::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain;
    typedef Eigen::Matrix<Scalar,3,3,Vector3LikePlain::Options> Matrix3;

    const Scalar t2 = v.squaredNorm();

    const Scalar t = math::sqrt(t2);
    if(t > TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      Scalar ct,st; SINCOS(t,&st,&ct);
      const Scalar alpha_vxvx = (1 - ct)/t2;
      const Scalar alpha_vx = (st)/t;
      Matrix3 res(alpha_vxvx * v * v.transpose());
      res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
      res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
      res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
      res.diagonal().array() += ct;

      return res;
    }
    else
    {
      const Scalar alpha_vxvx = Scalar(1)/Scalar(2) - t2/24;
      const Scalar alpha_vx = Scalar(1) - t2/6;
      Matrix3 res(alpha_vxvx * v * v.transpose());
      res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
      res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
      res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
      res.diagonal().array() += Scalar(1) - t2/2;

      return res;
    }
  }

  /// \brief Same as \ref log3
  ///
  /// \param[in] R the rotation matrix.
  /// \param[out] theta the angle value.
  ///
  /// \return The angular velocity vector associated to the rotation matrix.
  ///
  template<typename Matrix3Like>
  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
  log3(const Eigen::MatrixBase<Matrix3Like> & R,
       typename Matrix3Like::Scalar & theta)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);

    typedef typename Matrix3Like::Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;

    static const Scalar PI_value = PI<Scalar>();

    Vector3 res;
    const Scalar tr = R.trace();
    if(tr > Scalar(3))       theta = 0; // acos((3-1)/2)
    else if(tr < Scalar(-1)) theta = PI_value; // acos((-1-1)/2)
    else                     theta = math::acos((tr - Scalar(1))/Scalar(2));
    assert(theta == theta && "theta contains some NaN"); // theta != NaN

    // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
    if (theta < PI_value - 1e-2)
    {
      const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())
                        ? theta / sin(theta)
                        : Scalar(1)) / Scalar(2);
      res(0) = t * (R (2, 1) - R (1, 2));
      res(1) = t * (R (0, 2) - R (2, 0));
      res(2) = t * (R (1, 0) - R (0, 1));
    }
    else
    {
      // 1e-2: A low value is not required since the computation is
      // using explicit formula. However, the precision of this method
      // is the square root of the precision with the antisymmetric
      // method (Nominal case).
      const Scalar cphi = cos(theta - PI_value);
      const Scalar beta  = theta*theta / ( Scalar(1) + cphi );
      Vector3 tmp((R.diagonal().array() + cphi) * beta);
      res(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
      res(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
      res(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
    }

    return res;
  }

  /// \brief Log: SO3 -> so3.
  ///
  /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
  ///
  /// \param[in] R The rotation matrix.
  ///
  /// \return The angular velocity vector associated to the rotation matrix.
  ///
  template<typename Matrix3Like>
  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
  log3(const Eigen::MatrixBase<Matrix3Like> & R)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, R, 3, 3);

    typename Matrix3Like::Scalar theta;
    return log3(R.derived(),theta);
  }

  ///
  /// \brief Derivative of \f$ \exp{r} \f$
  /// \f[
  ///     \frac{\sin{||r||}}{||r||}                       I_3
  ///   - \frac{1-\cos{||r||}}{||r||^2}                   \left[ r \right]_x
  ///   + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T
  /// \f]
  ///
  template<typename Vector3Like, typename Matrix3Like>
  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
             const Eigen::MatrixBase<Matrix3Like> & Jexp)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r   , 3, 1);
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);

    Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
    typedef typename Matrix3Like::Scalar Scalar;

    Scalar n2 = r.squaredNorm(),a,b,c;
    Scalar n = math::sqrt(n2);

    if (n < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      a =   Scalar(1)           - n2/Scalar(6);
      b = - Scalar(1)/Scalar(2) - n2/Scalar(24);
      c =   Scalar(1)/Scalar(6) - n2/Scalar(120);
    }
    else
    {
      Scalar n_inv = Scalar(1)/n;
      Scalar n2_inv = n_inv * n_inv;
      Scalar cn,sn; SINCOS(n,&sn,&cn);

      a = sn*n_inv;
      b = - (1-cn)*n2_inv;
      c = n2_inv * (1 - a);
    }

    Jout.diagonal().setConstant(a);

    Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1);
    Jout(0,2) =  b*r[1]; Jout(2,0) = -Jout(0,2);
    Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2);

    Jout.noalias() += c * r * r.transpose();
  }

  template<typename Scalar, typename Vector3Like, typename Matrix3Like>
  void Jlog3(const Scalar & theta,
             const Eigen::MatrixBase<Vector3Like> & log,
             const Eigen::MatrixBase<Matrix3Like> & Jlog)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like,  log, 3, 1);
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jlog, 3, 3);

    Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);

    if (theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      const Scalar alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
      Jout.noalias() = alpha * log * log.transpose();

      Jout.diagonal().array() += Scalar(0.5) * (2 - theta*theta / Scalar(6));

      // Jlog += r_{\times}/2
      addSkew(0.5 * log, Jlog);
    }
    else
    {
      // Jlog = alpha I
      Scalar ct,st; SINCOS(theta,&st,&ct);
      const Scalar st_1mct = st/(Scalar(1)-ct);

      const Scalar alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
      Jout.noalias() = alpha * log * log.transpose();

      Jout.diagonal().array() += Scalar(0.5) * (theta*st_1mct);

      // Jlog += r_{\times}/2
      addSkew(0.5 * log, Jlog);
    }
  }

  template<typename Matrix3Like1, typename Matrix3Like2>
  void Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
             const Eigen::MatrixBase<Matrix3Like2> & Jlog)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like1,    R, 3, 3);
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like2, Jlog, 3, 3);

    typedef typename Matrix3Like1::Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;

    Scalar t;
    Vector3 w(log3(R,t));
    Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
  }

  ///
  /// \brief Exp: se3 -> SE3.
  ///
  /// Return the integral of the input twist during time 1.
  ///
  /// \param[in] nu The input twist.
  ///
  /// \return The rigid transformation associated to the integration of the twist during time 1.
  ///
  template<typename MotionDerived>
  SE3Tpl<typename MotionDerived::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
  exp6(const MotionDense<MotionDerived> & nu)
  {
    typedef typename MotionDerived::Scalar Scalar;
    enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options };

    typedef SE3Tpl<Scalar,Options> SE3;

    SE3 res;
    typename SE3::LinearType & trans = res.translation();
    typename SE3::AngularType & rot = res.rotation();

    const typename MotionDerived::ConstAngularType & w = nu.angular();
    const typename MotionDerived::ConstLinearType & v = nu.linear();

    Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
    const Scalar t2 = w.squaredNorm();
    const Scalar t = math::sqrt(t2);
    Scalar ct,st; SINCOS(t,&st,&ct);
    const Scalar inv_t2 = Scalar(1)/t2;

    alpha_wxv = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                       Scalar(1)/Scalar(2) - t2/24,
                                       (Scalar(1) - ct)*inv_t2);

    alpha_v = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                     Scalar(1) - t2/6,
                                     (st)/t);

    alpha_w = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                     (Scalar(1)/Scalar(6) - t2/120),
                                     (Scalar(1) - alpha_v)*inv_t2);

    diagonal_term = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                           Scalar(1) - t2/2,
                                           ct);

    // Linear
    trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));

    // Rotational
    rot.noalias() = alpha_wxv * w * w.transpose();
    rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
    rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
    rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
    rot.diagonal().array() += diagonal_term;

    return res;
  }

  /// \brief Exp: se3 -> SE3.
  ///
  /// Return the integral of the input spatial velocity during time 1.
  ///
  /// \param[in] v The twist represented by a vector.
  ///
  /// \return The rigid transformation associated to the integration of the twist vector during time 1..
  ///
  template<typename Vector6Like>
  SE3Tpl<typename Vector6Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
  exp6(const Eigen::MatrixBase<Vector6Like> & v)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);

    MotionRef<const Vector6Like> nu(v.derived());
    return exp6(nu);
  }

  /// \brief Log: SE3 -> se3.
  ///
  /// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
  ///
  /// \param[in] M The rigid transformation.
  ///
  /// \return The twist associated to the rigid transformation during time 1.
  ///
  template <typename Scalar, int Options>
  MotionTpl<Scalar,Options>
  log6(const SE3Tpl<Scalar,Options> & M)
  {
    typedef SE3Tpl<Scalar,Options> SE3;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef typename SE3::Vector3 Vector3;

    typename SE3::ConstAngularRef R = M.rotation();
    typename SE3::ConstLinearRef p = M.translation();

    Scalar t;
    Vector3 w(log3(R,t)); // t in [0,π]
    const Scalar t2 = t*t;
    Scalar alpha, beta;
    if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
      beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
    }
    else
    {
      Scalar st,ct; SINCOS(t,&st,&ct);
      alpha = t*st/(Scalar(2)*(Scalar(1)-ct));
      beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
    }

    return Motion(alpha * p - 0.5 * w.cross(p) + beta * w.dot(p) * w,
                  w);
  }

  /// \brief Log: SE3 -> se3.
  ///
  /// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
  ///
  /// \param[in] R The rigid transformation represented as an homogenous matrix.
  ///
  /// \return The twist associated to the rigid transformation during time 1.
  ///
  template<typename Matrix4Like>
  MotionTpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options>
  log6(const Eigen::MatrixBase<Matrix4Like> & M)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix4Like, M, 4, 4);

    SE3Tpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options> m(M);
    return log6(m);
  }

  /// \brief Derivative of exp6
  /// Computed as the inverse of Jlog6
  template<typename MotionDerived, typename Matrix6Like>
  void Jexp6(const MotionDense<MotionDerived>     & nu,
             const Eigen::MatrixBase<Matrix6Like> & Jexp)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);

    typedef typename MotionDerived::Scalar Scalar;
    typedef typename MotionDerived::Vector3 Vector3;
    typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
    Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);

    const typename MotionDerived::ConstLinearType  & v = nu.linear();
    const typename MotionDerived::ConstAngularType & w = nu.angular();
    const Scalar t2 = w.squaredNorm();
    const Scalar t = math::sqrt(t2);

    // Matrix3 J3;
    // Jexp3(w, J3);
    Jexp3(w, Jout.template bottomRightCorner<3,3>());
    Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();

    Scalar beta, beta_dot_over_theta;
    if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
      beta_dot_over_theta = Scalar(1)/Scalar(360);
    }
    else
    {
      const Scalar tinv = Scalar(1)/t,
                   t2inv = tinv*tinv;
      Scalar st,ct; SINCOS (t, &st, &ct);
      const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));

      beta = t2inv - st*tinv*inv_2_2ct;
      beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
        (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
    }

    Vector3 p (Jout.template topLeftCorner<3,3>().transpose() * v);
    Scalar wTp (w.dot (p));
    Matrix3 J (alphaSkew(.5, p) +
          (beta_dot_over_theta*wTp)                *w*w.transpose()
          - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
          + wTp * beta                             * Matrix3::Identity()
          + beta                                   *w*p.transpose());

    Jout.template topRightCorner<3,3>().noalias() =
      - Jout.template topLeftCorner<3,3>() * J;
    Jout.template bottomLeftCorner<3,3>().setZero();
  }

  /** \brief Derivative of log6
   *  \f[
   *  \left(\begin{array}{cc}
   *  \text{Jlog3}(R) & J * \text{Jlog3}(R) \\
   *            0     &     \text{Jlog3}(R) \\
   *  \end{array}\right)
   *  \f]
   *  where
   *  \f[
   *  \def\rot{R}
   *  \begin{eqnarray}
   *  J &=&
   *  \left.\frac{1}{2}[\mathbf{p}]_{\times} + \dot{\beta} (||r||) \frac{\rot^T\mathbf{p}}{||r||}\rot\rot^T
   *  - (||r||\dot{\beta} (||r||) + 2 \beta(||r||)) \mathbf{p}\rot^T\right.\\
   *  &&\left. + \rot^T\mathbf{p}\beta (||r||)I_3 + \beta (||r||)\rot\mathbf{p}^T\right.
   *  \end{eqnarray}
   *  \f]
   *  and
   *  \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f]
   */
  template<typename Scalar, int Options, typename Matrix6Like>
  void Jlog6(const SE3Tpl<Scalar, Options> & M,
             const Eigen::MatrixBase<Matrix6Like> & Jlog)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jlog, 6, 6);

    typedef SE3Tpl<Scalar,Options> SE3;
    typedef typename SE3::Vector3 Vector3;
    Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);

    typename SE3::ConstAngularRef R = M.rotation();
    typename SE3::ConstLinearRef p = M.translation();

    Scalar t;
    Vector3 w(log3(R,t));

    // value is decomposed as following:
    // value = [ A, B;
    //           C, D ]
    typedef Eigen::Block<Matrix6Like,3,3> Block33;
    Block33 A = value.template topLeftCorner<3,3>();
    Block33 B = value.template topRightCorner<3,3>();
    Block33 C = value.template bottomLeftCorner<3,3>();
    Block33 D = value.template bottomRightCorner<3,3>();

    Jlog3(t, w, A);
    D = A;

    const Scalar t2 = t*t;
    Scalar beta, beta_dot_over_theta;
    if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
      beta_dot_over_theta = Scalar(1)/Scalar(360);
    }
    else
    {
      const Scalar tinv = Scalar(1)/t,
                   t2inv = tinv*tinv;
      Scalar st,ct; SINCOS (t, &st, &ct);
      const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));

      beta = t2inv - st*tinv*inv_2_2ct;
      beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
        (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
    }

    Scalar wTp = w.dot(p);

    Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p);
    // C can be treated as a temporary variable
    C.noalias() = v3_tmp * w.transpose();
    C.noalias() += beta * w * p.transpose();
    C.diagonal().array() += wTp * beta;
    addSkew(.5*p,C);

    B.noalias() = C * A;
    C.setZero();
  }
} // namespace pinocchio

#include "pinocchio/spatial/explog-quaternion.hpp"

#endif //#ifndef __spatial_explog_hpp__


/*  Example code adapted from adept-1.0 advection example

    Copyright (C) 2012-2013 Robin Hogan and the University of Reading

    Contact email address: r.j.hogan@reading.ac.uk

    This file is part of the Adept library.

    This library is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <cmath>
#include <iostream>
#include <boost/format.hpp>
#include <vector>

#include <adept.h>

#define NX 100
using namespace adept;

// Lax-Wendroff scheme applied to linear advection
void
lax_wendroff(int nt, double c, const adouble q_init[NX], adouble q[NX])
{
    preallocate_statements((nt+1)*NX*3);
    preallocate_operations((nt+1)*NX*7);
    adouble flux[NX-1];                        // Fluxes between boxes
    for (int i=0; i<NX; i++) {
        q[i] = q_init[i]; // Initialize q
    }
    for (int j=0; j<nt; j++)
    {
        for (int i=0; i<NX-1; i++) {
            flux[i] = 0.5*c*(q[i]+q[i+1]+c*(q[i]-q[i+1]));
        }
        for (int i=1; i<NX-1; i++) {
            q[i] += flux[i-1]-flux[i];
        }
        q[0] = q[NX-2];
        q[NX-1] = q[1];          // Treat boundary conditions
    }
}

// Toon advection scheme applied to linear advection
void
toon(int nt, double c, const adouble q_init[NX], adouble q[NX])
{
    preallocate_statements((nt+1)*NX*3);
    preallocate_operations((nt+1)*NX*9);
    adouble flux[NX-1];                        // Fluxes between boxes
    for (int i=0; i<NX; i++) {
        q[i] = q_init[i]; // Initialize q
    }
    for (int j=0; j<nt; j++) {                 // Main loop in time
        for (int i=0; i<NX-1; i++) {
            flux[i] = (exp(c*log(q[i]/q[i+1]))-1.0)
                * q[i]*q[i+1] / (q[i]-q[i+1]);
        }
        for (int i=1; i<NX-1; i++) {
            q[i] += flux[i-1]-flux[i];
        }
        q[0] = q[NX-2]; q[NX-1] = q[1];          // Treat boundary
                                                 // conditions
    }
}

int
main(int argc, char** argv)
{
    double pi = 4.0*atan(1.0);
    double q_init[NX];

    int nt = 100;
    int nr = 500;
    double dt = 0.125;
    adept::Stack adept_stack;

    for (int i = 0; i < NX; i++) {
        q_init[i] = (0.5+0.5*sin((i*2.0*pi)/(NX-1.5)))+0.0001;
    }

    for (int j = 0; j < nr; j++)
    {
        adouble adept_q_init[NX];
        adouble adept_q[NX];

        adept::set_values(adept_q_init, NX, q_init);
        adept_stack.new_recording();
        toon(nt, dt, adept_q_init, adept_q);

        adept_stack.independent(adept_q_init, NX);
        adept_stack.dependent(adept_q, NX);

        double jacobian[NX*NX];
        adept_stack.jacobian(jacobian);

        if (j == 1) {
            for (int p = 0; p < 5; ++p) {
                std::cout << "  ";
                for (int q = 0; q < 5; ++q) {
                    std::cout << boost::format("%.16e") % jacobian[p + q*NX] << " ";
                }
                std::cout << std::endl;
            }
        }
    }

    return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd ones = MatrixXd::Ones(3,3);
VectorXcd eivals = ones.eigenvalues();
cout << "The eigenvalues of the 3x3 matrix of ones are:" << endl << eivals << endl;

  return 0;
}


#include "tetrahedron.h"

#include <Eigen/Eigen>

Tetrahedron::Tetrahedron(const Eigen::MatrixXd& V, const Eigen::VectorXi& indices, const Eigen::VectorXi& faces, double shear, double bulk)
: FiniteElement(V, indices, faces, shear, bulk)
{
	// Compute inverse initial deformation
	Rotation dX;
	const auto root = _X.row(3);
	dX << _X.row(0) - root, _X.row(1) - root, _X.row(2) - root;
	_invdX = dX.inverse();

	// Compute volume
	_volume = dX.determinant() / 6;
	if (_volume < 0.0)
	{
		std::cout << "Negative volume, invalid configuration" << std::endl;
	}
}

Tetrahedron::~Tetrahedron()
{

}

double Tetrahedron::compute_U(const Eigen::MatrixXd& E) const
{
	const double trE = E.trace();
	const double trSqr = E.cwiseProduct(E).sum();
	return (_shear / 2 * trE * trE + _bulk * trSqr) * _volume;
}

Eigen::MatrixXd Tetrahedron::compute_F(const Eigen::MatrixXd& x) const
{
	Rotation du;
	const auto& root = x.row(3);
	du << x.row(0) - root, x.row(1) - root, x.row(2) - root;
	return du * _invdX;
}

Eigen::MatrixXd Tetrahedron::compute_E(const Eigen::MatrixXd& F) const
{
	return (F.transpose() * F - Rotation::Identity()) / 2;
}

Eigen::MatrixXd Tetrahedron::compute_dUdE(const Eigen::MatrixXd& E) const
{
	Eigen::MatrixXd I = Rotation::Identity();
	return (_shear * E.trace() * I + 2 * _bulk * E) * _volume;
}

Eigen::MatrixXd Tetrahedron::compute_dEdF(const Eigen::MatrixXd& F) const
{
	return  F.transpose();
}

Eigen::MatrixXd Tetrahedron::compute_dFdx(const Eigen::MatrixXd& x, int32_t i, int32_t j) const
{
	Rotation dF = Rotation::Zero();
	if (i < 3)
	{
		dF(i, j) = 1.0;
	}
	else
	{
		dF.col(j) = Vec3(-1.0, -1.0, -1.0);
	}
	return dF * _invdX;
}

Eigen::MatrixXd Tetrahedron::compute_ddFddx(const Eigen::MatrixXd& x) const
{
	return _invdX;
}

Eigen::MatrixXd Tetrahedron::compute_ddEddF(const Eigen::MatrixXd& F) const
{
	return Eigen::Matrix3d::Identity();
}

Tensor Tetrahedron::compute_ddUddE(const Eigen::MatrixXd& E) const
{
	Tensor out(E.rows(), E.cols(), Eigen::Matrix3d::Zero());

	// Edge/Corner entries
	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < E.cols(); j++)
		{
			out(i, j)(i, j) += 2 * _bulk * _volume;
		}
	}

	// Diagonal entries
	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < E.cols(); j++)
		{
			out(i, i)(j, j) += _shear * _volume;
		}
	}

	return out;
}


#include "smooth_alg.hpp"

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Sparse>

#include <Eigen/QR>
#include <Eigen/LU>
#include <Eigen/SparseLU>

// -----------------------------------------------------------------------------

typedef Eigen::Triplet<double, int> Triplet;
/// declares a column-major sparse matrix type of double
typedef Eigen::SparseMatrix<double> Sparse_mat;

// -----------------------------------------------------------------------------

/// @return A sparse representation of the normalized Laplacian
/// list[ith_row][list of columns] = Triplet(ith_row, jth_column, matrix value)
static
std::vector<std::vector<Triplet>>
get_normalized_laplacian(const std::vector< Vec3 >& vertices,
                         const std::vector< std::vector<int> >& edges )
{
    unsigned nv = unsigned(vertices.size());
    std::vector<std::vector<Triplet>> mat_elemts(nv);
    for(int i = 0; i < nv; ++i)
        mat_elemts[i].reserve(10);

    for(int i = 0; i < nv; ++i)
    {
        const Vec3 c_pos = vertices[i];

        //get laplacian
        double sum = 0.;
        int nb_edges = edges[i].size();
        for(int e = 0; e < nb_edges; ++e)
        {
            int next_edge = (e + 1           ) % nb_edges;
            int prev_edge = (e + nb_edges - 1) % nb_edges;

            Vec3 v1 = c_pos                 - vertices[edges[i][prev_edge]];
            Vec3 v2 = vertices[edges[i][e]] - vertices[edges[i][prev_edge]];
            Vec3 v3 = c_pos                 - vertices[edges[i][next_edge]];
            Vec3 v4 = vertices[edges[i][e]] - vertices[edges[i][next_edge]];

            double cotan1 = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
            double cotan2 = (v3.dot(v4)) / (1e-6 + (v3.cross(v4)).norm() );

            double w = (cotan1 + cotan2)*0.5;
            sum += w;
            mat_elemts[i].push_back( Triplet(i, edges[i][e], w) );
        }

        for( Triplet& t : mat_elemts[i] )
            t = Triplet( t.row(), t.col(), t.value() / sum);

        mat_elemts[i].push_back( Triplet(i, i, -1.0) );
    }
    return mat_elemts;
}

// -----------------------------------------------------------------------------

static
std::vector< std::vector<float> >
get_cotan_weights(const std::vector< Vec3 >& vertices,
                  const std::vector< std::vector<int> >& edges)
{
    unsigned nv = unsigned(vertices.size());
    std::vector< std::vector<float> > weights(nv);

    for(int i = 0; i < nv; ++i)
    {
        const Vec3 c_pos = vertices[i];
        double sum = 0.;
        int nb_edges = edges[i].size();
        weights[i].resize( nb_edges );
        for(int e = 0; e < nb_edges; ++e)
        {
            int next_edge = (e + 1           ) % nb_edges;
            int prev_edge = (e + nb_edges - 1) % nb_edges;

            Vec3 v1 = c_pos                 - vertices[edges[i][prev_edge]];
            Vec3 v2 = vertices[edges[i][e]] - vertices[edges[i][prev_edge]];
            Vec3 v3 = c_pos                 - vertices[edges[i][next_edge]];
            Vec3 v4 = vertices[edges[i][e]] - vertices[edges[i][next_edge]];

            double cotan1 = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
            double cotan2 = (v3.dot(v4)) / (1e-6 + (v3.cross(v4)).norm() );

            double w = (cotan1 + cotan2)*0.5;
            weights[i][e] = w;
        }
    }
    return weights;
}

//------------------------------------------------------------------------------

std::vector< Vec3 >
smooth_iterative(const std::vector< Vec3 >& in_vertices,
                 const std::vector< std::vector<int> >& edges,
                 int nb_iter,
                 float alpha)
{
    unsigned nb_vertices = unsigned(in_vertices.size());
    std::vector< std::vector<float> > cotan_weights = get_cotan_weights(in_vertices, edges);

    std::vector< Vec3 > buffer_vertices( nb_vertices );
    std::vector< Vec3 > source = in_vertices;

    if(nb_iter == 0){
        return in_vertices;
    }

    Vec3* src_vertices = source.data();
    Vec3* dst_vertices = buffer_vertices.data();
    for(int k = 0; k < nb_iter; k++)
    {
        for( int i = 0; i < nb_vertices; i++)
        {
#if 0
            if( _topo->is_vert_on_side( i ) ){
                dst_vertices[i] = src_vertices[i];
                continue;
            }
#endif

            Vec3 cog(0.f);
            float sum = 0.f;
            size_t nb_neighs = edges[i].size();
            for(size_t n = 0; n < nb_neighs; n++)
            {
                Vert_idx neigh = edges[i][n];
                float w = cotan_weights[i][n];
                cog += src_vertices[neigh] * w;
                sum += w;
            }
            float t = alpha;
            dst_vertices[i] = (cog / sum) * t + src_vertices[i] * (1.f - t);

        }

        std::swap(dst_vertices, src_vertices);
    }

    return (nb_iter%2 == 1) ? buffer_vertices : source;
}

//------------------------------------------------------------------------------

//explicit
std::vector< Vec3 >
explicit_laplacian_smoothing(const std::vector< Vec3 >& in_vertices,
                             const std::vector< std::vector<int> >& edges,
                             int nb_iter,
                             float alpha)
{
    unsigned nb_vertices = unsigned(in_vertices.size());

    std::vector<Eigen::VectorXd> xyz;
    std::vector<Eigen::VectorXd> rhs;

    xyz.resize(3, Eigen::VectorXd::Zero(nb_vertices));
    rhs.resize(3, Eigen::VectorXd::Zero(nb_vertices));

    for(int i = 0; i < nb_vertices; ++i)
    {
        Vec3 pos = in_vertices[i];
        xyz[0][i] = pos.x;
        xyz[1][i] = pos.y;
        xyz[2][i] = pos.z;
    }

    // Build laplacian
    std::vector<std::vector<Triplet>> mat_elemts = get_normalized_laplacian(in_vertices, edges);
    Eigen::SparseMatrix<double> L(nb_vertices, nb_vertices);
    std::vector<Triplet> triplets;
    triplets.reserve(nb_vertices * 10);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            triplets.push_back( elt );

    L.setFromTriplets(triplets.begin(), triplets.end());

    Eigen::SparseMatrix<double> I = Eigen::MatrixXd::Identity(nb_vertices, nb_vertices).sparseView();
    L = I + L*alpha;

    //L = L*L*L*L*L*L*L*L*L*L;
    rhs = xyz;
    for(int n = 0; n < nb_iter; n++){
        for(int k = 0; k < 3; k++){
            xyz[k] = (L * xyz[k]);
        }
    }

    std::vector< Vec3 > out_verts(nb_vertices);
    for(int i = 0; i < nb_vertices; ++i){
        Vec3 v;
        v.x = xyz[0][i];
        v.y = xyz[1][i];
        v.z = xyz[2][i];
        out_verts[i] = v;
    }

    return out_verts;
}

//------------------------------------------------------------------------------

//implicit
std::vector< Vec3 >
implicit_laplacian_smoothing(const std::vector< Vec3 >& in_vertices,
                             const std::vector< std::vector<int> >& edges,
                             int nb_iter,
                             float alpha)
{
    unsigned nb_vertices = unsigned(in_vertices.size());

    std::vector<Eigen::VectorXd> xyz;
    std::vector<Eigen::VectorXd> rhs;

    xyz.resize(3, Eigen::VectorXd::Zero(nb_vertices));
    rhs.resize(3, Eigen::VectorXd::Zero(nb_vertices));

    for(int i = 0; i < nb_vertices; ++i)
    {
        Vec3 pos = in_vertices[i];
        rhs[0][i] = pos.x;
        rhs[1][i] = pos.y;
        rhs[2][i] = pos.z;
    }

    // Build laplacian
    std::vector<std::vector<Triplet>> mat_elemts = get_normalized_laplacian(in_vertices, edges);
    Eigen::SparseMatrix<double> L(nb_vertices, nb_vertices);
    std::vector<Triplet> triplets;
    triplets.reserve(nb_vertices * 10);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            triplets.push_back( elt );

    L.setFromTriplets(triplets.begin(), triplets.end());

    Eigen::SparseMatrix<double> I = Eigen::MatrixXd::Identity(nb_vertices, nb_vertices).sparseView();
    L = I - L*alpha;

    L = L*L*L;

    // Solve for x, y, z
    Eigen::SparseLU<Sparse_mat> solver;
    solver.compute( L );

    for(int k = 0; k < 3; k++){
        xyz[k] = solver.solve(rhs[k]);
    }

    std::vector< Vec3 > out_verts(nb_vertices);
    for(int i = 0; i < nb_vertices; ++i){
        Vec3 v;
        v.x = xyz[0][i];
        v.y = xyz[1][i];
        v.z = xyz[2][i];
        out_verts[i] = v;
    }

    return out_verts;
}


#include <exception>
#include <NTL/ZZ.h>
#include "RSA.h"

const char *KeyError::what() {
	return "Tried to decrypt using a public key.";
}

// Input: m, integer representation of plaintext
// key, the RSA public key
// Output: c, integer representation of ciphertext
void encrypt(NTL::ZZ& c, const NTL::ZZ& m, const RSAkey& key) {
	PowerMod(c, m, key.get_param("e"), key.get_param("n"));
}

// Input: c, integer representation of ciphertext
// key, the RSA private key
// Output: m, integer representation of plaintext
void decrypt(NTL::ZZ& m, const NTL::ZZ& c, const RSAkey& key) {
	if(key.is_private()) {
		PowerMod(m, c, key.get_param("d"), key.get_param("n"));
	} else {
		throw KeyError();
	}
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "MathExtensions.h"

#include <boost/math/special_functions/factorials.hpp>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>

#include <stdexcept>

const double cppqedutils::PI(M_PI);
const double cppqedutils::SQRTPI(M_SQRTPI);
const double cppqedutils::EULER(M_E);

int cppqedutils::sign(double x) {return GSL_SIGN(x);}
int cppqedutils::fcmp(double x, double y, double eps) {return gsl_fcmp(x,y,eps);}

double cppqedutils::sqr(double x) {return gsl_pow_2(x);}

double cppqedutils::sqrAbs(dcomp x) {return sqr(real(x))+sqr(imag(x));} // saves the sqrt

double cppqedutils::fact(unsigned n)
{
  if (n>GSL_SF_FACT_NMAX) throw std::out_of_range("Factorial of"+std::to_string(n));
  return gsl_sf_fact(n);
}

double cppqedutils::choose(unsigned n, unsigned m)
{
  return gsl_sf_choose(n,m);
}

dcomp cppqedutils::coherentElement(unsigned long n, dcomp alpha)
{
  using namespace boost::math;
  return n ? n<max_factorial<double>::value ? pow(alpha,n)/sqrt(factorial<double>(n))
                                            : pow(2*n*PI,-.25)*pow(alpha/sqrt(n/EULER),n)
           : 1.;
}


//
//  Eigenvalues of the radial part of a 3D exponential well
//
// compile with:
//     g++ -std=c++11 exp_well_3d.cpp -o exp_well_3d -I$HOME/miniconda3/include/ -O3
//
// using the a Miniconda install of boost for the headers

#include <cstdio>
#include <iostream>
#include <cmath>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

typedef boost::array< long double , 2 > vec_type;
typedef runge_kutta_cash_karp54< vec_type > error_stepper_type;
typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
typedef runge_kutta4< vec_type > stepper;

struct ode{

    long double m;
    long double energy;
    long double a;
    long double k;
    long double l;     // angluar quantum number

    void operator()( const vec_type &vec , vec_type &dvecdx , double r)
    {
        dvecdx[0] = vec[1];
        dvecdx[1] =  -2.0*vec[1]/r - (2*m*(energy - k*(exp(a*r) - 1.0)) - (l*(l+1)/(r*r)))*vec[0];
    }
};

struct nothing{

    void operator()( const vec_type &vec , const double r){

    }
};

void print_exp_well_psi_final(double k, double a, double m){
    controlled_stepper_type controlled_stepper;

    // Energy sweep range
    double min_energy = 0.000001;
    double max_energy = 10 * 0.00094;    // kBT = 0.00094 Ha

    int l_max = 50;                       // Maximum value of the l qunatum number

    double r_init = 0.001;               // Where to start the propogation from
    double r_final = 1.7;                // Where to end   '   '  as there is a divergence at r=0

    int n_steps = 20000;

    for (int l = 0; l <= l_max; l++ ){

        double prev_psi_final = 0.0;         // Final value of Psi i.e. Psi(r_final) for each eigenvalue

        for (int i = 0; i < n_steps; i++){

            double energy = min_energy + ((max_energy - min_energy) * i) / n_steps;

            vec_type vec = { 1.0 , 1E-20 }; // initial conditions
            integrate_adaptive( make_controlled< error_stepper_type >( 1.0e-10 , 1.0e-10 ),
                               ode {m , energy, a, k, static_cast<long double>(l)},
                               vec,
                               r_init,
                               r_final,
                               1E-10,
                               nothing {}
                               );

            // If the product of the previous Psi(r_final) and the current one is negative
            // there must be a zero between them, i.e. a normalaisable WF
            if (prev_psi_final * vec[0] < 0){
                // cout << energy << endl;
                cout << energy << '\t' << l << endl;

            }

            // Reset the previous Psi(r_final)
            prev_psi_final = vec[0];

        } // Energy
    }  // l
}

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

    freopen("eigenvals_co2.txt","w", stdout);
    cout << "Eigenvalue / Ha    l number" << endl;

    double k_co2 = 0.00220873;                   // prefactor for the exponential well

    double k = 0.0001;
    double a = 2.832 * 0.529177;             // exponent in the expoential well
    double m = 48 * 1822.888486;             // mass in atomic units from amu

    print_exp_well_psi_final(k_co2, a, m);

    return 0;
}


/*
 * Copyright 2018-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <eigen-osqp/CSCMatrix.h>
#include <eigen-osqp/OSQP.h>
#include <iostream>

int main()
{
    int nrVar = 6;
    int nrConstr = 5;
    auto inf = std::numeric_limits<double>::infinity();

    Eigen::SparseMatrix<double> Q(nrVar, nrVar);
    Q.setIdentity();
    Eigen::SparseMatrix<double> A(nrConstr, nrVar);
    std::vector<Eigen::Triplet<double>> nz = {
        { 0, 0, 1. },
        { 0, 1, -1. },
        { 0, 2, 1. },
        { 0, 4, 3. },
        { 0, 5, 1. },
        { 1, 0, -1. },
        { 1, 2, -3. },
        { 1, 3, -4. },
        { 1, 4, 5. },
        { 1, 5, 6. },
        { 2, 0, 2. },
        { 2, 1, 5. },
        { 2, 2, 3. },
        { 2, 4, 1. },
        { 3, 1, 1. },
        { 3, 3, 1. },
        { 3, 4, 2. },
        { 3, 5, -1. },
        { 4, 0, -1. },
        { 4, 2, 2. },
        { 4, 3, 1. },
        { 4, 4, 1. },
    };

    A.setFromTriplets(nz.cbegin(), nz.cend());

    Eigen::VectorXd c(nrVar);
    Eigen::VectorXd AL(nrConstr);
    Eigen::VectorXd AU(nrConstr);
    Eigen::VectorXd XL(nrVar);
    Eigen::VectorXd XU(nrVar);
    c << 1., 2., 3., 4., 5., 6.;
    AL << 1., 2., 3., -inf, -inf;
    AU << 1., 2., 3., -1., 2.5;
    XL << -1000., -10000., 0., -1000., -1000., -1000.;
    XU << 10000., 100., 1.5, 100., 100., 1000.;

    Eigen::OSQP qp;

    qp.problem(nrVar, nrConstr);
    bool success = qp.solve(Q, c, A, AL, AU, XL, XU);

    Eigen::VectorXd result = qp.result();

    std::cout << "Problem:"
              << "\n\tminimize 0.5*x'*Q*x + c'*x"
              << "\n\twith     AL <= A <= AU"
              << "\n\t         XL <= x <= XU"
              << "\n\nQ:\n"
              << Q
              << "\nc:\n"
              << c.transpose()
              << "\nA:\n"
              << A
              << "\nAL:\n"
              << AL.transpose()
              << "\nAU:\n"
              << AU.transpose()
              << "\nXL:\n"
              << XL.transpose()
              << "\nXU:\n"
              << XU.transpose()
              << "\n\n\nSolution:\n"
              << result.transpose() << std::endl;

    std::cout << "\nPress enter to quit" << std::endl;
    std::cin.get();
}


#include "ugl/random/normal_distribution.h"

#include <cmath>
#include <random>

#include <Eigen/Eigenvalues>

#include "random_engine.h"

namespace ugl::random
{

ugl::MatrixD create_transform(const ugl::MatrixD& covariance)
{
    Eigen::SelfAdjointEigenSolver<ugl::MatrixD> eigen_solver(covariance);
    return eigen_solver.eigenvectors() * eigen_solver.eigenvalues().cwiseSqrt().asDiagonal();
}

NormalDistribution<1>::NormalDistribution(double mean, double variance)
    : mean_(mean)
    , variance_(variance)
    , stddev_(std::sqrt(variance))
{
}

double NormalDistribution<1>::sample() const
{
    std::normal_distribution distribution{mean_, stddev_};
    return distribution(internal::rng);
}

double NormalDistribution<1>::sample(double mean, double variance)
{
    std::normal_distribution distribution{mean, std::sqrt(variance)};
    return distribution(internal::rng);
}

} // namespace ugl::random


//
// Helper utilities for simulators.
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh> // Definition of dynamics function.

#include <Eigen/Dense>

#include <functional>

namespace simulators
{

template<int dim>
using Vector = Eigen::Matrix<double, dim, 1>;

// Wrapper struct so we can use template paramters.
template<int xdim, int udim>
struct RK4
{
using Dynamics = std::function<Vector<xdim>(const Vector<xdim> &x, const Vector<udim> &u)>;

// 4th order Runge-Kutta integration of an ODE given by dynamics.
static Vector<xdim> rk4(const double dt,
        const Vector<xdim> state,
        const Vector<udim> control,
        const Dynamics &dynamics)
{

    // This cannot be changed
    constexpr double RK4_INTEGRATION_CONSTANT = 1.0/6.0;

    // Formula from: http://mathworld.wolfram.com/Runge-KuttaMethod.html
    const Vector<xdim> k1 = dynamics(state, control);
    const Vector<xdim> k2 = dynamics(state + 0.5*dt*k1, control);
    const Vector<xdim> k3 = dynamics(state + 0.5*dt*k2, control);
    const Vector<xdim> k4 = dynamics(state + dt*k3, control);

    const Vector<xdim> result = state + RK4_INTEGRATION_CONSTANT*dt*(k1 + 2.0*(k2 + k3) + k4);
    return result;
}

};

constexpr double INTEGRATION_FREQUENCY = 5;
constexpr double MIN_INTEGRATION_DT = 1e-2;

Eigen::VectorXd step(const Eigen::VectorXd &state, const Eigen::VectorXd &control,
                     const double dt, const ilqr::DynamicsFunc &dynamics,
                     const double min_integration_dt = MIN_INTEGRATION_DT,
                     const double integration_frequency  = INTEGRATION_FREQUENCY
                     );

} // namespace simulators


// Boost.Geometry
// Unit Test

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

#include <sstream>

#include "test_formula.hpp"
#include "inverse_cases.hpp"

#include <boost/geometry/formulas/vincenty_inverse.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/andoyer_inverse.hpp>

void check_inverse(std::string const& name,
                   expected_results const& results,
                   bg::formula::result_inverse<double> const& result,
                   expected_result const& expected,
                   expected_result const& reference,
                   double reference_error)
{
    std::stringstream ss;
    ss << "(" << results.p1.lon << " " << results.p1.lat << ")->(" << results.p2.lon << " " << results.p2.lat << ")";

    check_one(name + "_d  " + ss.str(),
              result.distance, expected.distance, reference.distance, reference_error);
    check_one(name + "_a  " + ss.str(),
              result.azimuth, expected.azimuth, reference.azimuth, reference_error, true);
    check_one(name + "_ra " + ss.str(),
              result.reverse_azimuth, expected.reverse_azimuth, reference.reverse_azimuth, reference_error, true);
    check_one(name + "_rl " + ss.str(),
              result.reduced_length, expected.reduced_length, reference.reduced_length, reference_error);
    check_one(name + "_gs " + ss.str(),
              result.geodesic_scale, expected.geodesic_scale, reference.geodesic_scale, reference_error);
}

void test_all(expected_results const& results)
{
    double const d2r = bg::math::d2r<double>();
    double const r2d = bg::math::r2d<double>();

    double lon1r = results.p1.lon * d2r;
    double lat1r = results.p1.lat * d2r;
    double lon2r = results.p2.lon * d2r;
    double lat2r = results.p2.lat * d2r;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::result_inverse<double> result_v, result_t, result_a;

    typedef bg::formula::vincenty_inverse<double, true, true, true, true, true> vi_t;
    result_v = vi_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_v.azimuth *= r2d;
    result_v.reverse_azimuth *= r2d;
    check_inverse("vincenty", results, result_v, results.vincenty, results.reference, 0.0000001);

    typedef bg::formula::thomas_inverse<double, true, true, true, true, true> th_t;
    result_t = th_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_t.azimuth *= r2d;
    result_t.reverse_azimuth *= r2d;
    check_inverse("thomas", results, result_t, results.thomas, results.reference, 0.00001);

    typedef bg::formula::andoyer_inverse<double, true, true, true, true, true> an_t;
    result_a = an_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_a.azimuth *= r2d;
    result_a.reverse_azimuth *= r2d;
    check_inverse("andoyer", results, result_a, results.andoyer, results.reference, 0.001);
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    return 0;
}


#include "CubicInterpolation/FindParameter.hpp"

#include <boost/math/tools/roots.hpp>
#include <cassert>

namespace cubic_splines {
namespace detail {
double _find_parameter(std::function<double(double)> const &f,
                        std::function<double(double)> const &df, double x_guess,
                        double lower, double upper) {
  assert(((void)"find_parameter call is not well defined", lower < upper));
  auto func = [&f, &df](double x) { return std::make_tuple(f(x), df(x)); };

  if (std::isnan(x_guess)) {
    auto bisec_tolerance = (upper - lower) * 1e-2;
    auto tol = [&bisec_tolerance](double min, double max) {
      return (std::abs(max - min) < bisec_tolerance) ? true : false;
    };
    std::tie(lower, upper) = boost::math::tools::bisect(f, lower, upper, tol);
    x_guess = (lower + upper) / 2;
    if (lower == upper)
      return lower;
  }

  return boost::math::tools::newton_raphson_iterate(func, x_guess, lower, upper, 20);
}
} // namespace detail
} // namespace cubic_splines


#include "NumUtils.h"

#include <NTL/RR.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <cassert>
#include <cmath>

#include "CZZ.h"

void NumUtils::sampleGauss(ZZX& res, const long& size, const double& stdev) {
	static double const Pi = 4.0 * atan(1.0);
	static long const bignum = 0xfffffff;
	res.SetLength(size);

	for (long i = 0; i < size; i+=2) {
		double r1 = (1 + RandomBnd(bignum)) / ((double)bignum + 1);
		double r2 = (1 + RandomBnd(bignum)) / ((double)bignum + 1);
		double theta=2 * Pi * r1;
		double rr= sqrt(-2.0 * log(r2)) * stdev;
		assert(rr < 8 * stdev); // sanity-check, no more than 8 standard deviations
		// Generate two Gaussians RV's, rounded to integers
		long x1 = (long) floor(rr * cos(theta) + 0.5);
		SetCoeff(res, i, x1);
		if(i + 1 < size) {
			long x2 = (long) floor(rr * sin(theta) + 0.5);
			SetCoeff(res, i + 1, x2);
		}
	}
}

void NumUtils::sampleHWT(ZZX& res, const long& size, const long& h) {
	res.SetLength(size);
	long idx = 0;
	ZZ tmp = RandomBits_ZZ(h);
	while(idx < h) {
		long i = RandomBnd(size);
		if(res.rep[i] == 0) {
			res.rep[i] = (bit(tmp, idx) == 0) ? ZZ(1) : ZZ(-1);
			idx++;
		}
	}
}

void NumUtils::sampleZO(ZZX& res, const long& size) {
	res.SetLength(size);
	ZZ tmp = RandomBits_ZZ(2 * size);
	for (long i = 0; i < size; ++i) {
		res.rep[i] = (bit(tmp, 2 * i) == 0) ? ZZ(0) : (bit(tmp, 2 * i + 1) == 0) ? ZZ(1) : ZZ(-1);
	}
}

void NumUtils::sampleBinary(ZZX& res, const long& size, const long& h) {
	res.SetLength(size);
	long idx = 0;
	while(idx < h) {
		long i = RandomBnd(size);
		if(res.rep[i] == 0) {
			res.rep[i] = ZZ(1);
			idx++;
		}
	}
}

void NumUtils::sampleBinary(ZZX& res, const long& size) {
	res.SetLength(size);
	ZZ tmp = RandomBits_ZZ(size);
	for (long i = 0; i < size; ++i) {
		res.rep[i] = (bit(tmp, i) == 0) ? ZZ(0) : ZZ(1);
	}
}

void NumUtils::sampleUniform2(ZZX& res, const long& size, const long& logBnd) {
	res.SetLength(size);
	for (long i = 0; i < size; i++) {
		res.rep[i] = RandomBits_ZZ(logBnd);
	}
}

void NumUtils::fftRaw(CZZ*& vals, const long& size, SchemeAux& aux, const bool& isForward) {
	for (long i = 1, j = 0; i < size; ++i) {
		long bit = size >> 1;
		for (; j >= bit; bit>>=1) {
			j -= bit;
		}
		j += bit;
		if(i < j) {
			swap(vals[i], vals[j]);
		}
	}
	if(isForward) {
		for (long len = 2; len <= size; len <<= 1) {
			long MoverLen = aux.M / len;
			for (long i = 0; i < size; i += len) {
				for (long j = 0; j < len / 2; ++j) {
					CZZ u = vals[i + j];
					CZZ v = vals[i + j + len / 2];
					RR tmp1 = to_RR(v.r) * (aux.ksiPowsr[j * MoverLen] + aux.ksiPowsi[j * MoverLen]);
					RR tmpr = tmp1 - to_RR(v.r + v.i) * aux.ksiPowsi[j * MoverLen];
					RR tmpi = tmp1 + to_RR(v.i - v.r) * aux.ksiPowsr[j * MoverLen];
					v.r = to_ZZ(tmpr);
					v.i = to_ZZ(tmpi);
					vals[i + j] = u + v;
					vals[i + j + len / 2] = u - v;
				}
			}
		}
	} else {
		for (long len = 2; len <= size; len <<= 1) {
			long MoverLen = aux.M / len;
			for (long i = 0; i < size; i += len) {
				for (long j = 0; j < len / 2; ++j) {
					CZZ u = vals[i + j];
					CZZ v = vals[i + j + len / 2];
					RR tmp1 = to_RR(v.r) * (aux.ksiPowsr[(len - j) * MoverLen] + aux.ksiPowsi[(len - j) * MoverLen]);
					RR tmpr = tmp1 - to_RR(v.r + v.i) * aux.ksiPowsi[(len - j) * MoverLen];
					RR tmpi = tmp1 + to_RR(v.i - v.r) * aux.ksiPowsr[(len - j) * MoverLen];
					v.r = to_ZZ(tmpr);
					v.i = to_ZZ(tmpi);
					vals[i + j] = u + v;
					vals[i + j + len / 2] = u - v;
				}
			}
		}
	}
}

void NumUtils::fft(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, true);
}

void NumUtils::fftInv(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, false);
	long logSize = log2(size);
	for (long i = 0; i < size; ++i) {
		vals[i] >>= logSize;
	}
}

void NumUtils::fftInvLazy(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, false);
}

void NumUtils::fftSpecial(CZZ*& vals, const long& size, SchemeAux& aux) {
	for (int i = 1, j = 0; i < size; ++i) {
		long bit = size >> 1;
		for (; j>=bit; bit>>=1) {
			j -= bit;
		}
		j += bit;
		if(i < j) {
			swap(vals[i], vals[j]);
		}
	}
	for (long len = 2; len <= size; len <<= 1) {
		long loglen = log2(len);
		long Mover2Len = aux.M / len / 2;
		for (long i = 0; i < size; i += len) {
			for (long j = 0; j < len / 2; ++j) {
				CZZ u = vals[i + j];
				CZZ v = vals[i + j + len / 2];
				RR tmp1 = to_RR(v.r) * (aux.ksiPowsr[(2 * j + 1) * Mover2Len] + aux.ksiPowsi[(2 * j + 1) * Mover2Len]);
				RR tmpr = tmp1 - to_RR(v.r + v.i) * aux.ksiPowsi[(2 * j + 1) * Mover2Len];
				RR tmpi = tmp1 + to_RR(v.i - v.r) * aux.ksiPowsr[(2 * j + 1) * Mover2Len];
				v.r = to_ZZ(tmpr);
				v.i = to_ZZ(tmpi);
				vals[i + j] = u + v;
				vals[i + j + len / 2] = u - v;
			}
		}
	}
}

void NumUtils::fftSpecialInv(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, false);
	long logsize = log2(size);
	long Mover2size = aux.M / size / 2;
	for (long i = 0; i < size; ++i) {
		RR tmp1 = to_RR(vals[i].r) * (aux.ksiPowsr[(2 * size - i) * Mover2size] + aux.ksiPowsi[(2 * size - i) * Mover2size]);
		RR tmpr = tmp1 - to_RR(vals[i].r + vals[i].i) * aux.ksiPowsi[(2 * size - i) * Mover2size];
		RR tmpi = tmp1 + to_RR(vals[i].i - vals[i].r) * aux.ksiPowsr[(2 * size - i) * Mover2size];
		vals[i].r = to_ZZ(tmpr);
		vals[i].i = to_ZZ(tmpi);
		vals[i] >>= logsize;
	}
}


#include "glog/logging.h"
#include <armadillo>

#include "pclem_math.h"

#include "matrix33.h"

namespace pclem {
    Matrix33::Matrix33() : values{} {}

    // The values are given row-major.
    Matrix33::Matrix33(const std::array<double,9>& values) : values(values) {}

    double Matrix33::get_element(const int& i, const int& j) const {
        return values[i*3 + j];
    }

    Matrix33 Matrix33::identity() {
        return Matrix33({1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0});
    }

    bool Matrix33::operator==(const Matrix33& other) const {
        for(int i=0; i < 9; i++) {
            if(!approximatelyEqual(values[i], other.values[i], 1e-10)) {
                return false;
            }
        }
        return true;
    }

    Vector3 Matrix33::get_column(int i) const {
        return Vector3(values[i], values[3+i], values[6+i]);
    }

    double Matrix33::trace() const {
        return values[0] + values[4] + values[8];
    }

    // Returns the eigenvalues in descending order. The first column
    // of the matrix is the eigenvector of the largest eigenvalue, and
    // so on.
    std::pair<Vector3, Matrix33> Matrix33::eigen_decomposition() const {

        VLOG(10) << "Extracting eigenvalues...";

        if(symetric(1e-10)) {
            return symetrical_eigen_decomposition();
        } else {
            return general_eigen_decomposition();
        }
    }

    std::pair<Vector3, Matrix33> Matrix33::symetrical_eigen_decomposition() const {
        arma::mat33 arma_cov_mat(values.data());
        arma::vec arma_eigvals;
        arma::mat arma_eigvecs;

        if(!arma::eig_sym(arma_eigvals, arma_eigvecs, arma_cov_mat)) {
            LOG(WARNING) << "Eigenvalues decomposition failed.";
        }

        Vector3 eigvals;
        std::array<double,9> eigvecs_values;
        for(int i=0; i < 3; i++) {
            eigvals[i] = arma_eigvals[2-i];

            for(int j=0; j < 3; j++) {
                eigvecs_values[i*3 + j] = arma_eigvecs(i,2-j);
            }
        }

        return std::make_pair(eigvals, Matrix33(eigvecs_values));
    }

    std::pair<Vector3, Matrix33> Matrix33::general_eigen_decomposition() const {
        arma::mat33 arma_cov_mat(values.data());
        arma::cx_vec arma_eigvals;
        arma::cx_mat arma_eigvecs;

        if(!arma::eig_gen(arma_eigvals, arma_eigvecs, arma_cov_mat)) {
            LOG(WARNING) << "Eigenvalues decomposition failed.";
        }

        Vector3 eigvals;
        std::array<double,9> eigvecs_values;
        for(int i=0; i < 3; i++) {
            eigvals[i] = arma_eigvals[2-i].real();

            for(int j=0; j < 3; j++) {
                eigvecs_values[i*3 + j] = arma_eigvecs(j,i).real();
            }
        }

        return std::make_pair(eigvals, Matrix33(eigvecs_values));
    }

    std::array<double,9> Matrix33::inverse() const {
        VLOG(11) << "Inverting matrix...";

        arma::mat33 arma_cov_mat(values.data());
        arma::mat33 arma_inv_of_cov = arma::pinv(arma_cov_mat + arma::eye(3,3));

        VLOG(11) << "PseudoInverse error " << arma::norm(arma_cov_mat*arma_inv_of_cov - arma::eye(3,3));

        std::array<double,9> inverse;
        for(auto i = 0; i < 3; i++) {
            for(auto j = 0; j < 3; j++) {
                inverse[i*3 + j] = arma_inv_of_cov(i,j);
            }
        }

        VLOG(11) << "Done inverting matrix.";
        return inverse;
    }

    double Matrix33::det() const {
        return get_element(0,0) * (get_element(1,1) * get_element(2,2) - get_element(2, 1) * get_element(1,2)) -
            get_element(0,1) * (get_element(1,0) * get_element(2,2) - get_element(1,2) * get_element(2,0)) +
            get_element(0,2) * (get_element(1,0) * get_element(2,1) + get_element(2,0) * get_element(1,1));
    }

    std::ostream& operator<<(std::ostream& os, const Matrix33& v) {
        for(int i=0; i < 8; i++) {
            os << v.values[i] << ",";
        }
        os << v.values[8];
    }

    void Matrix33::set_element(const int& i, const int& j, const double value) {
        values[i*3 + j] = value;
    }

    std::ofstream& operator<<(std::ofstream& ofs, const Matrix33& v) {
        for(int i=0; i < 8; i++) {
            ofs << v.values[i] << ",";
        }
        ofs << v.values[8];

        return ofs;
    }

    bool Matrix33::symetric(double epsilon) const {
        for(int i = 0; i < 3; i++) {
            for(int j = 0; j < 3; j++) {
                if(!approximatelyEqual(get_element(i, j), get_element(i, j), epsilon)) {
                        return false;
                }
            }
        }

        return true;
    }

    Matrix33 Matrix33::zeros() {
        return Matrix33({0.0});
    }
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <dpMM/basemeasure.hpp>
#include <dpMM/niw.hpp>

/*
 * NIW base measure; integrates over Normal distribution parameters
 */
template<typename T>
class NiwMarginalized : public BaseMeasure<T>
{
public:
  NIW<T> niw0_;
  NIW<T> niw_;

  NiwMarginalized(const NIW<T>& niw);
  ~NiwMarginalized();

  virtual baseMeasureType getBaseMeasureType() const {return(NIW_MARGINALIZED); }

  virtual BaseMeasure<T>* copy();

  T logLikelihood(const Matrix<T,Dynamic,1>& x) const;

  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
      uint32_t k);

  T logPdfUnderPrior() const;

  void print() const ;
  virtual uint32_t getDim() const {return(uint32_t(niw_.D_));};
};

/*
 * NIW base measure; samples Normal distribution parameters
 */
template<typename T>
class NiwSampled : public BaseMeasure<T>
{
public:
  NIW<T> niw0_;
  Normal<T> normal_;

  NiwSampled(const NIW<T>& niw);
  NiwSampled(const NIW<T>& niw, const Normal<T> &normal);
  ~NiwSampled();

  virtual baseMeasureType getBaseMeasureType() const {return(NIW_SAMPLED); }

  virtual BaseMeasure<T>* copy();
  virtual NiwSampled<T>* copyNative();

  T logLikelihood(const Matrix<T,Dynamic,1>& x) const;
  T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  // assumes vector [N, sum(x), flatten(sum(outer(x,x)))]
  T logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const;
  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  void posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x, const
      VectorXu& z, uint32_t k);
  void posteriorFromSS(const vector<Matrix<T,Dynamic,1> >&x, const
      VectorXu& z, uint32_t k);
  void posteriorFromSS(const Matrix<T,Dynamic,1> &x);
  void sample();

  T logPdfUnderPrior() const;
  T logPdfUnderPriorMarginalized() const;
  T logPdfUnderPriorMarginalizedMerged(const shared_ptr<NiwSampled<T> >& other) const;

  T logPdfUnderPriorMarginalized(const Matrix<T,Dynamic,1>& x) {return 0.;};

  virtual NiwSampled<T>* merge(const NiwSampled<T>& other);
  void fromMerge(const NiwSampled<T>& niwA, const NiwSampled<T>& niwB);

  void print() const;
  virtual uint32_t getDim() const {return(uint32_t(normal_.D_));};

  const Matrix<T,Dynamic,Dynamic>& scatter() const {return niw0_.scatter();};
  const Matrix<T,Dynamic,1>& mean() const {return niw0_.mean();};
  T count() const {return niw0_.count();};
//  T& count() {return niw0_.count_;};
  const Matrix<T,Dynamic,1>& getMean() const {return normal_.mu_;};

  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return normal_.Sigma();};
private:

};

typedef NiwSampled<double> NiwSampledd;
typedef NiwSampled<float> NiwSampledf;

// ---------------------------------------------------------------------------

template<typename T>
NiwSampled<T>::NiwSampled(const NIW<T>& niw)
  : niw0_(niw), normal_(niw0_.sample())
{};

template<typename T>
NiwSampled<T>::NiwSampled(const NIW<T>& niw, const Normal<T> &normal)
 : niw0_(niw), normal_(normal)
{};

template<typename T>
NiwSampled<T>::~NiwSampled()
{};

template<typename T>
BaseMeasure<T>* NiwSampled<T>::copy()
{
  NiwSampled<T>* niwSampled = new NiwSampled<T>(niw0_);
  niwSampled->normal_ = normal_;
  return niwSampled;
};

template<typename T>
NiwSampled<T>* NiwSampled<T>::copyNative()
{
  NiwSampled<T>* niwSampled = new NiwSampled<T>(niw0_);
  niwSampled->normal_ = normal_;
  return niwSampled;
};

template<typename T>
T NiwSampled<T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
//  normal_.print();
  T logLike = normal_.logPdf(x);
//  cout<<x.transpose()<<" -> " <<logLike<<endl;
//  cout<<x.transpose()<<" -> " <<normal_.logPdfSlower(x)<<endl;
  return logLike;
};

// assumes vector [N, sum(x), flatten(sum(outer(x,x)))]
template<typename T>
T NiwSampled<T>::logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const
{
//  normal_.print();
  uint32_t D = niw0_.D_;
  T count = x(0);
  Matrix<T,Dynamic,1> mean(D);
  if(count>0)
	  mean = x.middleRows(1,D)/count;
  else
	  mean = Matrix<T,Dynamic,1>::Zero(D); //this should not matter since everything gets multiplied by 0 counts

  //NOTE: Eigen::Map does not like const data, so this cast is needed to strip const data from input
  //alternatively the input could be changed to non-const, but this is cleaner from the outside
  T* datPtr = const_cast<T*>(&(x.data()[(D+1)]));
  Matrix<T,Dynamic,Dynamic> scatter =
    Map<Matrix<T,Dynamic,Dynamic> >(datPtr,D,D);
  scatter -= (mean*mean.transpose())*count;

  T logLike = normal_.logPdf(scatter,mean,count);
  //  cout<<x.transpose()<<" -> " <<logLike<<endl;
  //  cout<<x.transpose()<<" -> " <<normal_.logPdfSlower(x)<<endl;
  return logLike;
};

template<typename T>
void NiwSampled<T>::posterior(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k)
{
  normal_ = niw0_.posterior(x,z,k).sample();
};

template<typename T>
void NiwSampled<T>::posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x,
	const VectorXu& z, uint32_t k)
{
	normal_ = niw0_.posterior(x,z,k).sample();
}

template<typename T>
void NiwSampled<T>::posteriorFromSS(const vector<Matrix<T,Dynamic,1> > &x, const VectorXu& z, uint32_t k)
{
	normal_ = niw0_.posteriorFromSS(x,z,k).sample();
}

template<typename T>
void NiwSampled<T>::posteriorFromSS(const Matrix<T,Dynamic,1> &x)
{
	normal_ = niw0_.posteriorFromSS(x).sample();
}

template<typename T>
T NiwSampled<T>::logPdfUnderPrior() const
{
  return niw0_.logPdf(normal_);
};

template<typename T>
T NiwSampled<T>::logPdfUnderPriorMarginalized() const
{
  // evaluates log pdf of sufficient statistics stored within niw0_
//  niw0_.print();
  return niw0_.logPdfMarginalized();
};

template<typename T>
T NiwSampled<T>::logPdfUnderPriorMarginalizedMerged(
    const shared_ptr<NiwSampled<T> >& other) const
{
  return niw0_.logPdfUnderPriorMarginalizedMerged(other->niw0_);
};

template<typename T>
void NiwSampled<T>::fromMerge(const NiwSampled<T>& niwA,
  const NiwSampled<T>& niwB)
{
  niw0_.fromMerge(niwA.niw0_,niwB.niw0_);
  normal_ = niw0_.posterior().sample();
};

template<typename T>
NiwSampled<T>* NiwSampled<T>::merge(const NiwSampled<T>& other)
{
  NiwSampled<T>* newNiw = this->copyNative();
  newNiw->niw0_.fromMerge(niw0_,other.niw0_);
  newNiw->sample();
  return newNiw;
};

template<typename T>
void NiwSampled<T>::sample()
{
  normal_ = niw0_.posterior().sample();
};

template<typename T>
void NiwSampled<T>::print() const
{
  niw0_.posterior().print();
  normal_.print();
};

// ----------------------------------------------------------------------------

template<typename T>
NiwMarginalized<T>::NiwMarginalized(const NIW<T>& niw)
  : niw0_(niw),niw_(niw)
{};

template<typename T>
NiwMarginalized<T>::~NiwMarginalized()
{};

template<typename T>
BaseMeasure<T>* NiwMarginalized<T>::copy()
{
  return new NiwMarginalized<T>(niw0_);
};

template<typename T>
T NiwMarginalized<T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
  return niw_.logProb(x);
};

template<typename T>
void NiwMarginalized<T>::posterior(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k)
{
  niw_ = niw0_.posterior(x,z,k);
};

template<typename T>
T NiwMarginalized<T>::logPdfUnderPrior() const
{
  // 0 since we integrate over parameters -> there is no parameters for which
  // we would want to eval a pdf
  return 0.0;
};

template<typename T>
void NiwMarginalized<T>::print() const
{
  niw0_.posterior().print();
};


#ifndef SHAPES_SPHERE_HPP
#define SHAPES_SPHERE_HPP

#include <math/point3d.hpp>

#include <boost/optional.hpp>

namespace math { struct ray3d; }

namespace shapes
{

struct intersection_info;

struct sphere
{
  math::point3d center;
  float radius;
};

inline bool operator==(sphere lhs, sphere rhs)
{
  return std::tie(lhs.center, lhs.radius) == std::tie(rhs.center, rhs.radius);
}

inline bool operator!=(sphere lhs, sphere rhs)
{
  return !(lhs == rhs);
}

// Check for ray-sphere intersection using geometric test.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
bool intersects(shapes::sphere shape, math::ray3d ray);

// Returns the closest intersection point between ray and sphere.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
boost::optional<shapes::intersection_info> closest_intersection(shapes::sphere shape, math::ray3d ray);

}

#endif


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#ifndef BOGUS_EIGEN_LINEAR_SOLVERS
#define BOGUS_EIGEN_LINEAR_SOLVERS

#include "../Utils/LinearSolverBase.hpp"

#include <Eigen/LU>
#include <Eigen/Cholesky>

namespace bogus {

template<typename Decomposition, typename RhsType>
struct EigenSolveResult
{
#if EIGEN_VERSION_AT_LEAST(3,2,90)
	  typedef const Eigen::Solve< Decomposition, RhsType > Type ;
#else
	  typedef Eigen::internal::solve_retval< Decomposition, RhsType > Type ;
#endif
};

template < typename Derived >
struct LinearSolverTraits< LU< Eigen::MatrixBase< Derived > > >
{
	typedef typename Derived::PlainObject MatrixType ;
	typedef Eigen::FullPivLU< MatrixType > FactType ;

	template < typename RhsT > struct Result {
		typedef typename EigenSolveResult< FactType, RhsT >::Type Type ;
	} ;
	template < typename RhsT >
	struct Result< Eigen::MatrixBase< RhsT > > {
		typedef typename Result< RhsT >::Type Type ;
	} ;
} ;

template < typename Derived >
struct LU< Eigen::MatrixBase< Derived > >
		: public LinearSolverBase< LU< Eigen::MatrixBase< Derived > > >
{
	typedef Eigen::MatrixBase< Derived > MatrixType ;
	typedef LinearSolverTraits< LU< MatrixType > > Traits ;

	LU() {}
	template< typename OtherDerived >
	explicit LU ( const Eigen::MatrixBase< OtherDerived >& mat )
		: m_fact( mat )
	{}

	template< typename OtherDerived >
	LU< Eigen::MatrixBase< Derived > >& compute ( const Eigen::MatrixBase< OtherDerived >& mat )
	{
		m_fact.compute( mat ) ;
		return *this ;
	}

	template < typename RhsT, typename ResT >
	void solve( const Eigen::MatrixBase< RhsT >& rhs, ResT& res ) const
	{
		res = m_fact.solve( rhs ) ;
	}

	template < typename RhsT >
	typename Traits::template Result< Eigen::MatrixBase< RhsT > >::Type
	solve( const Eigen::MatrixBase< RhsT >& rhs ) const
	{
		return m_fact.solve( rhs ) ;
	}

  private:
	typename Traits::FactType m_fact ;
} ;

template < typename Scalar, int Rows, int Cols = Rows, int Options = 0 >
struct DenseLU : public LU< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Cols, Options > > >
{
	DenseLU() {}
	template< typename OtherDerived >
	explicit DenseLU ( const Eigen::MatrixBase< OtherDerived >& mat )
		: LU< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Cols, Options > > >( mat )
	{}
} ;

template < typename Derived >
struct LinearSolverTraits< LDLT< Eigen::MatrixBase< Derived > > >
{
	typedef typename Derived::PlainObject MatrixType ;
	typedef Eigen::LDLT< MatrixType > FactType ;

	template < typename RhsT > struct Result {
		typedef typename EigenSolveResult< FactType, RhsT >::Type Type ;
	} ;
	template < typename RhsT >
	struct Result< Eigen::MatrixBase< RhsT > > {
		typedef typename Result< RhsT >::Type Type ;
	} ;
} ;

template < typename Derived >
struct LDLT< Eigen::MatrixBase< Derived > >
		: public LinearSolverBase< LDLT< Eigen::MatrixBase< Derived > > >
{
	typedef Eigen::MatrixBase< Derived > MatrixType ;
	typedef LinearSolverTraits< LDLT< MatrixType > > Traits ;

	LDLT() {}
	template< typename OtherDerived >
	explicit LDLT ( const Eigen::MatrixBase< OtherDerived >& mat )
		: m_fact( mat )
	{}

	template< typename OtherDerived >
	LDLT< Eigen::MatrixBase< Derived > >& compute ( const Eigen::MatrixBase< OtherDerived >& mat )
	{
		m_fact.compute( mat ) ;
		return *this ;
	}

	template < typename RhsT, typename ResT >
	void solve( const Eigen::MatrixBase< RhsT >& rhs, ResT& res ) const
	{
		res = m_fact.solve( rhs ) ;
	}

	template < typename RhsT >
	typename Traits::template Result< Eigen::MatrixBase< RhsT > >::Type
	solve( const Eigen::MatrixBase< RhsT >& rhs ) const
	{
		return m_fact.solve( rhs ) ;
	}

  private:
	typename Traits::FactType m_fact ;
} ;

template < typename Scalar, int Rows, int Options = 0 >
struct DenseLDLT : public LDLT< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Rows, Options > > >
{
	DenseLDLT() {}
	template< typename OtherDerived >
	explicit DenseLDLT ( const Eigen::MatrixBase< OtherDerived >& mat )
		: LDLT< Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Rows, Options > > >( mat )
	{}
} ;

template < typename Derived, typename RhsT >
typename LinearSolverTraits< Derived >::template Result< Eigen::MatrixBase< RhsT > >::Type operator*
	( const LinearSolverBase< Derived >& solver, const Eigen::MatrixBase< RhsT >& rhs )
{
  return solver.solve( rhs ) ;
}

} //namespace bogus

#endif


/*
 * lorenz.hpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * 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)
 */

#ifndef LORENZ_HPP_
#define LORENZ_HPP_

#include <boost/array.hpp>

struct lorenz
{
    template< class state_type >
    void inline operator()( const state_type &x , state_type &dxdt , const double t ) const
    {
        const double sigma = 10.0;
        const double R = 28.0;
        const double b = 8.0 / 3.0;
        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = x[0]*x[1] - b * x[2];
    }
};

#endif /* LORENZ_HPP_ */


// SPDX-License-Identifier: Apache-2.0
// Copyright 2021 - 2021, the Anboto author and contributors
#include <Core/Core.h>
#include <Functions4U/Functions4U.h>
#include <Eigen/Eigen.h>
#include "Permutations.h"

namespace Upp {

using namespace Eigen;

Buffer<Buffer<int>> PermutationsWithRepetition(int nVals, int nOptionsVal) {
	Buffer<Buffer<int>> list;

	int num = int(pow(nOptionsVal, nVals));
	list.Alloc(nVals);
	for (int ip = 0; ip < nVals; ++ip) {
		list[ip].Alloc(num);
		int nrep = int(pow(nOptionsVal, ip));
		int val = 0, irep = 0;
		for (int i = 0; i < num; ++i) {
			list[ip][i] = val;
			irep++;
			if (irep >= nrep) {
				irep = 0;
				val++;
				if (val >= nOptionsVal)
					val = 0;
			}
		}
	}
	return list;
}

}