/**
 * @file transformations.cpp
 * @brief Convenience transformation matrices written in 3D homogeneous coordinates (i.e. matrices shape is 4x4).
 *        Under the hood, Eigen matrices are used.
 *
 * @author Daniel Calderón
 * @license MIT
*/

#include "transformations.h"

#include <cmath>
#include <Eigen/Geometry>

namespace Grafica
{
namespace Transformations
{

Matrix4f identity()
{
    return Matrix4f::Identity();
}

Matrix4f uniformScale(Coord s)
{
    return (Matrix4f() <<
        s, 0, 0, 0,
        0, s, 0, 0,
        0, 0, s, 0,
        0, 0, 0, 1).finished();
}

Matrix4f scale(Coord sx, Coord sy, Coord sz)
{
    return (Matrix4f() <<
        sx, 0, 0, 0,
        0, sy, 0, 0,
        0, 0, sz, 0,
        0, 0, 0, 1).finished();
}

Matrix4f rotationX(Coord theta_radians)
{
    Coord sin_theta = std::sin(theta_radians);
    Coord cos_theta = std::cos(theta_radians);

    return (Matrix4f() <<
        1,         0,          0, 0,
        0, cos_theta, -sin_theta, 0,
        0, sin_theta,  cos_theta, 0,
        0,         0,          0, 1).finished();
}

Matrix4f rotationY(Coord theta_radians)
{
    Coord sin_theta = std::sin(theta_radians);
    Coord cos_theta = std::cos(theta_radians);

    return (Matrix4f() <<
         cos_theta, 0, sin_theta, 0,
                 0, 1,         0, 0,
        -sin_theta, 0, cos_theta, 0,
                 0, 0,         0, 1).finished();
}

Matrix4f rotationZ(Coord theta_radians)
{
    Coord sin_theta = std::sin(theta_radians);
    Coord cos_theta = std::cos(theta_radians);

    return (Matrix4f() <<
        cos_theta, -sin_theta, 0, 0,
        sin_theta,  cos_theta, 0, 0,
                0,          0, 1, 0,
                0,          0, 0, 1).finished();
}

Matrix4f rotationA(Coord theta_radians, Vector3f axis)
{
    Coord s = std::sin(theta_radians);
    Coord c = std::cos(theta_radians);

    axis.normalize();

    Coord x = axis[0];
    Coord y = axis[1];
    Coord z = axis[2];

    return (Matrix4f() <<
        // First row
        c + (1 - c) * x * x,
        (1 - c) * x * y - s * z,
        (1 - c) * x * z + s * y,
        0,
        // Second row
        (1 - c) * x * y + s * z,
        c + (1 - c) * y * y,
        (1 - c) * y * z - s * x,
        0,
        // Third row
        (1 - c) * x * z - s * y,
        (1 - c) * y * z + s * x,
        c + (1 - c) * z * z,
        0,
        // Fourth row
        0,0,0,1).finished();
}

Matrix4f translate(Coord tx, Coord ty, Coord tz)
{
    return (Matrix4f() <<
        1, 0, 0, tx,
        0, 1, 0, ty,
        0, 0, 1, tz,
        0, 0, 0,  1).finished();
}

Matrix4f shearing(Coord xy, Coord yx, Coord xz, Coord zx, Coord yz, Coord zy)
{
    return (Matrix4f() <<
         1, xy, xz, 0,
        yx,  1, yz, 0,
        zx, zy,  1, 0,
         0,  0,  0, 1).finished();
}

Matrix4f frustum(Coord left, Coord right, Coord bottom, Coord top, Coord near, Coord far)
{
    Coord r_l = right - left;
    Coord t_b = top - bottom;
    Coord f_n = far - near;

    return (Matrix4f() <<
        // First row
        2 * near / r_l,
        0,
        (right + left) / r_l,
        0,
        // Second row
        0,
        2 * near / t_b,
        (top + bottom) / t_b,
        0,
        // Third row
        0,
        0,
        -(far + near) / f_n,
        -2 * near * far / f_n,
        // Fourth row
        0, 0, -1, 0).finished();
}

Matrix4f perspective(Coord fovy, Coord aspect, Coord near, Coord far)
{
    Coord halfHeight = std::tan(M_PI * fovy / 360) * near;
    Coord halfWidth = halfHeight * aspect;

    return frustum(-halfWidth, halfWidth, -halfHeight, halfHeight, near, far);
}

Matrix4f ortho(Coord left, Coord right, Coord bottom, Coord top, Coord near, Coord far)
{
    Coord r_l = right - left;
    Coord t_b = top - bottom;
    Coord f_n = far - near;

    return (Matrix4f() <<
        // First row
        2 / r_l,
        0,
        0,
        -(right + left) / r_l,
        // Second row
        0,
        2 / t_b,
        0,
        -(top + bottom) / t_b,
        // Third row
        0,
        0,
        -2 / f_n,
        -(far + near) / f_n,
        // Fourth row
        0, 0, 0, 1).finished();
}

Matrix4f lookAt(Vector3f const& eye, Vector3f const& at, Vector3f const& up)
{
    Vector3f forward = at - eye;
    forward.normalize();

    Vector3f side = forward.cross(up);
    side.normalize();

    Vector3f newUp = side.cross(forward);
    newUp.normalize();

    return (Matrix4f() <<
            side[0],       side[1],    side[2], -side.dot(eye),
            newUp[0],     newUp[1],   newUp[2], -newUp.dot(eye),
            -forward[0], -forward[1], -forward[2], forward.dot(eye),
            0,0,0,1).finished();
}

} // Transformations
} // Grafica


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main(){
    Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0,0,1)).toRotationMatrix();
    Quaterniond q(R);
    Sophus::SO3d SO3_R(R);
    Sophus::SO3d SO3_q(q);

    cout << "SO3 from matrix: \n" << SO3_R.matrix() << endl;
    cout << "SO3 from quaternion: \n" << SO3_q.matrix() << endl;

    Vector3d so3 = SO3_R.log();
    cout << "so3 = " << so3.transpose() << endl;
    cout << "so3 hat = \n" << Sophus::SO3d::hat(so3) << endl;
    cout << "so3 hat vee = " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

    Vector3d update_so3(1e-4,0,0);
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
    cout << "SO3 updated = \n" << SO3_updated.matrix();

    cout << "------------------------------------------" << endl;

    Vector3d t(1,0,0);
    Sophus::SE3d SE3R_t(R,t);
    Sophus::SE3d SE3q_t(q,t);
    cout << "SE3 from R,t = \n" << SE3R_t.matrix() << endl;
    cout << "SE3 from q,t = \n" << SE3q_t.matrix() << endl;

    Eigen::Matrix<double,6,1> se3 = SE3R_t.log();
    cout << "se3 = " << se3.transpose() << endl;

    cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
    cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

    Eigen::Matrix<double,6,1> se3_update;
    se3_update << 1e-4,0,0,0,0,0;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(se3_update)*SE3R_t;
    cout << "SE3 updated = \n" << SE3_updated.matrix() << endl;

}


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

/// 本程序演示sophus的基本用法

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

  // 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // 或者四元数
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3d SO3_q(q);              // 也可以通过四元数构造
  // 二者是等价的
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // 相对的，vee为反对称到向量
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // 增量扰动模型的更新
  Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;
  // 对SE(3)操作大同小异
  Vector3d t(1, 0, 0);           // 沿X轴平移1
  Sophus::SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
  Sophus::SE3d SE3_qt(q, t);            // 从q,t构造SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // 李代数se(3) 是一个六维向量，方便起见先typedef一下
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  // 同样的，有hat和vee两个算符
  cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // 最后，演示一下更新
  Vector6d update_se3; //更新量
  update_se3.setZero();
  update_se3(0, 0) = 1e-4d;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}


#ifndef R_1_PUBLIC_GRAPH_SPECTRAL_H_
#define R_1_PUBLIC_GRAPH_SPECTRAL_H_

#include <Eigen/Dense>
#include "Graph.hpp"

#include <algorithm>

namespace R1 {
	class SpectralGraph : public Graph {
	public:
		virtual ~SpectralGraph() {}

		SpectralGraph(int NVertex) : Graph(NVertex) {

		}

		SpectralGraph() {

		}

		//
		// spectral graph properties
		//

		Eigen::VectorXf spectrum() {
			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eg1(adjacency_matrix);
			return eg1.eigenvalues();
		}

		// The energy of G was first defined by Gutman in 1978 as
		// the sum of the absolute values of its eigenvalues
		double energy() {
			Eigen::MatrixXf::EigenvaluesReturnType eg = adjacency_matrix.eigenvalues();
			double energySum = 0;

			for (int i = 0; i < eg.rows(); i++)	{
				for (int j = 0; j < eg.cols(); j++)	{
					energySum += sqrt(pow(eg(i, j).real(), 2)  + pow(eg(i, j).imag(), 2));
				}
			}

			return energySum;
		}

		// Two graphs are called isospectral or cospectral if the
		// adjacency matrices of the graphs have equal multisets
		// of eigenvalues.
		static bool isospectral(SpectralGraph g1, SpectralGraph g2)
		{
			return g1.spectrum() == g2.spectrum();
		}

		// Two graphs are called isospectral or cospectral if the
		// adjacency matrices of the graphs have equal multisets
		// of eigenvalues.
		static bool isospectral(SpectralGraph g1, SpectralGraph g2, double epsilon)	{
			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eg1(g1.adjacency_matrix);
			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eg2(g2.adjacency_matrix);

			if (eg1.info() != Eigen::Success) {
				return false;
			}

			if (eg2.info() != Eigen::Success) {
				return false;
			}

			Eigen::VectorXf normalizedEg1 = eg1.eigenvalues().normalized();
			Eigen::VectorXf normalizedEg2 = eg2.eigenvalues().normalized();

			if (normalizedEg1.cols() != normalizedEg2.cols()) {
				return false;
			}

			if (normalizedEg1.rows() != normalizedEg2.rows()) {
				return false;
			}

			for (int i = 0; i < normalizedEg1.rows(); i++)	{
				for (int j = 0; j < normalizedEg1.cols(); j++)	{
					if (abs(normalizedEg1(i, j) - normalizedEg2(i, j)) > epsilon) {
						return false;
					}
				}
			}

			return true;
		}

		//////////////////////////////////////////////////////////////////////////
		//								Laplacian
		//////////////////////////////////////////////////////////////////////////

		float laplacian(int u, int v) {

			float du = degree(u);

			if (u == v && du != 0) {
				return du;
			} else if (adjacency_matrix(u, v) > 0.0f) {
				return -adjacency_matrix(u, v);
			}

			return 0.0f;
		}

		float normalizedLaplacian(int u, int v) {

			float du = degree(u);

			if (u == v && du != 0) {
				return 1.0;
			}
			else if (adjacency_matrix(u, v) > 0.0) {

				float dv = degree(v);

				return -adjacency_matrix(u, v) / sqrt(du * dv);
			}

			return 0.0f;
		}

		Eigen::MatrixXf getLaplacianMatrix() {
			Eigen::MatrixXf degree_matrix(n_vertex, n_vertex);
			degree_matrix.setZero();

			for (int i = 0; i < n_vertex; i++)	{
				degree_matrix(i, i) = degree(i);
			}

			return degree_matrix - adjacency_matrix;
		}

		Eigen::MatrixXf getAjacencyMatrix() {
			return adjacency_matrix;
		}

		Eigen::MatrixXf getLaplacianMatrix_obsolete() {
			Eigen::MatrixXf laplacian_matrix(n_vertex, n_vertex);

			for (int i = 0; i < n_vertex; i++)	{
				for (int j = 0; j < n_vertex; j++)	{
					laplacian_matrix(i, j) = laplacian(i, j);
				}

				if (n_vertex > 10 && (i % (n_vertex / 10) == 0))
					std::cout << "LAPLACIAN : " << (100 * i) / n_vertex << std::endl;
			}

			return laplacian_matrix;
		}

		Eigen::MatrixXf getNormalizedLaplacianMatrix() {
			Eigen::MatrixXf degree_matrix(n_vertex, n_vertex);
			degree_matrix.setZero();

			for (int i = 0; i < n_vertex; i++)	{
				degree_matrix(i, i) = 1.0f / (sqrt(degree(i)));
			}

			return Eigen::MatrixXf::Identity(n_vertex, n_vertex) - degree_matrix * adjacency_matrix * degree_matrix;
		}

		Eigen::MatrixXf getNormalizedLaplacianMatrix_obsolete() {
			Eigen::MatrixXf laplacian_matrix(n_vertex, n_vertex);

			for (int i = 0; i < n_vertex; i++)	{
				for (int j = 0; j < n_vertex; j++)	{
					laplacian_matrix(i, j) = normalizedLaplacian(i, j);
				}
			}

			return laplacian_matrix;
		}

		Eigen::VectorXcf laplacianSpectrum() {

			Eigen::MatrixXf L = getLaplacianMatrix();
			Eigen::EigenSolver<Eigen::MatrixXf> eg1(L);
			return eg1.eigenvalues();
		}

		Eigen::VectorXcf normalizedLaplacianSpectrum() {

			Eigen::MatrixXf L = getLaplacianMatrix();
			Eigen::EigenSolver<Eigen::MatrixXf> eg1(L);
			return eg1.eigenvalues();
		}

		Eigen::MatrixXcf laplacianEigenVectors() {

			Eigen::MatrixXf L = getLaplacianMatrix();

			Eigen::EigenSolver<Eigen::MatrixXf> eg1(L);
			return eg1.eigenvectors();
		}

		Eigen::MatrixXcf normalizedLaplacianEigenVectors() {

			Eigen::MatrixXf L = getNormalizedLaplacianMatrix();

			Eigen::EigenSolver<Eigen::MatrixXf> eg1(L);
			return eg1.eigenvectors();
		}

		// The Fiedler vector of the graph Laplacian
		Eigen::VectorXf getFiedlerVector(bool normalize = false) {

			Eigen::MatrixXf L;
			if (!normalize)
				L = getLaplacianMatrix();
			else
				L = getNormalizedLaplacianMatrix();

			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eg1;
			eg1.compute(L, Eigen::DecompositionOptions::ComputeEigenvectors);

			Eigen::VectorXf spec = eg1.eigenvalues();

			std::cout << spec << std::endl;

			float m0 = FLT_MAX;
			float m1 = FLT_MAX;

			int m0_index = 0;
			int m1_index = 0;

			for (int i = 0; i < spec.size(); i++)
			{
				if (spec(i) < m0)
				{
					m0 = spec(i);
					m0_index = i;
				}
				else if (spec(i) <= m1)
				{
					m1 = spec(i);
					m1_index = i;
				}
			}

			return eg1.eigenvectors().col(m1_index);
		}

		void getSortedFiedlerVector(std::vector<float>& fiedlerVector, bool normalize = false)
		{
			Eigen::VectorXf fv = getFiedlerVector(normalize);

			for (int i = 0; i < fv.size(); i++)
			{
				fiedlerVector.push_back(fv(i));
			}

			std::sort(fiedlerVector.begin(), fiedlerVector.end());
		}

		static float FiedlerVectorSimilarity(SpectralGraph& sg1, SpectralGraph& sg2)
		{
			std::vector<float> fiedlerVector1;
			std::vector<float> fiedlerVector2;

			sg1.getSortedFiedlerVector(fiedlerVector1);
			sg2.getSortedFiedlerVector(fiedlerVector2);

			size_t n = std::max(fiedlerVector1.size(), fiedlerVector2.size());

			Eigen::VectorXf fv1(n);
			Eigen::VectorXf fv2(n);

			fv1.setZero();
			fv2.setZero();

			for (size_t i = 0; i < n; i++)
			{
				if (i < fiedlerVector1.size())
				{
					fv1(i) = fiedlerVector1[i];
				}

				if (i < fiedlerVector2.size())
				{
					fv2(i) = fiedlerVector2[i];
				}
			}

			return fv1.dot(fv2) / (fv1.norm() * fv2.norm());
		}

		void saveDotFile(std::string filename)
		{
			std::ofstream out(filename);
			out << "graph spectralgraph {\n";
			out << "overlap = scale;\n";

			auto cluster = spectralClusters(false);

			std::unordered_map<int, int> nodeCulster;

			for (size_t i = 0; i < cluster.clusters.size(); i++)
			{
				nodeCulster[cluster.clusters[i].id] = cluster.clusters[i].cluster_id;

				if (cluster.clusters[i].cluster_id == 0)
					out << cluster.clusters[i].id << "[color = blue];" << std::endl;
				else if (cluster.clusters[i].cluster_id == 1)
					out << cluster.clusters[i].id << "[color = green];" << std::endl;
				else
					out << cluster.clusters[i].id << "[color = red];" << std::endl;
			}

			for (int i = 0; i < n_vertex; i++)
			{
				for (int j = 0; j < i; j++)
				{
					if (is_connected(i, j))
					{
						out << i << "--" << j << ";" << std::endl;
					}
				}
			}

			out << "}\n";

			out.close();
		}

		ClusterCollection spectralClusters(bool normalize = false)
		{
			ClusterCollection cc;

			cc.size = 2;

			Eigen::VectorXf fv = getFiedlerVector(normalize);

			std::cout << fv << std::endl;

			for (int i = 0; i < fv.size(); i++)
			{
				ClusteredNode cnode;
				cnode.id = i;

				if (fabs(fv(i)) < 0.001) {
					cnode.cluster_id = 0;
				} else if (fv(i) > 0.0) {
					cnode.cluster_id = 1;
				} else {
					cnode.cluster_id = 2;
				}

				cc.clusters.push_back(cnode);
			}

			return cc;
		}

		/*static bool isospectralLaplacian(SpectralGraph g1, SpectralGraph g2, double epsilon) {

			Eigen::VectorXf normalizedEg1 = g1.laplacianSpectrum().normalized();
			Eigen::VectorXf normalizedEg2 = g2.laplacianSpectrum().normalized();

			if (normalizedEg1.cols() != normalizedEg2.cols()) {
				return false;
			}

			if (normalizedEg1.rows() != normalizedEg2.rows()) {
				return false;
			}

			for (int i = 0; i < normalizedEg1.rows(); i++)	{
				for (int j = 0; j < normalizedEg1.cols(); j++)	{
					if (abs(normalizedEg1(i, j) - normalizedEg2(i, j)) > epsilon) {
						return false;
					}
				}
			}

			return true;
		}

		static double distance(SpectralGraph g1, SpectralGraph g2) {
			Eigen::VectorXf normalizedEg1 = g1.laplacianSpectrum().normalized();
			Eigen::VectorXf normalizedEg2 = g2.laplacianSpectrum().normalized();

			double dist = 0.0;

			if (normalizedEg1.cols() != normalizedEg2.cols()) {
				return 0.0;
			}

			if (normalizedEg1.rows() != normalizedEg2.rows()) {
				return 0.0;
			}

			for (int i = 0; i < normalizedEg1.rows(); i++)	{
				for (int j = 0; j < normalizedEg1.cols(); j++)	{
					dist += abs(normalizedEg1(i, j) - normalizedEg2(i, j));
				}
			}

			return dist;
		}*/
	};
}

#endif


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

int main(int argc, char **argv)
{
    using namespace boost::numeric::ublas;
    using std::cout;
    using std::endl;

    int xlen = 5;      //(int) *argv[0];
    int ylen = 5;      //(int) *argv[1];
    long double xmin = -10;      //(long double) *argv[2];
    long double xmax = 10;    //(long double) *argv[3];
    long double ymin = -10;    //(long double) *argv[4];
    long double ymax = 10;     //(long double) *argv[5];
    long double dx = (xmax - xmin)/(xlen-1);
    long double dy = (ymax - ymin)/(ylen-1);

    matrix<std::complex<long double> > cplane(ylen,xlen);

    for (unsigned r = 0; r < cplane.size1(); r++)
    {
        for (unsigned c = 0; c < cplane.size2(); c++)
        {
            cplane(r,c) = std::complex<long double>(xmin + c*dx, ymin + r*dy);
        }
    }
    for (unsigned r = 0; r < cplane.size1(); r++)
    {
        for (unsigned c = 0; c < cplane.size2(); c++)
        {
            cout << xmin + c*dx << " + " << ymin + r*dy << "i" << "    ";
        }
        cout << endl;
    }

}


// laplace_example.cpp

// Copyright Paul A. Bristow 2008, 2010.

// 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)

// Example of using laplace (& comparing with normal) distribution.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[laplace_example1
/*`
First we need some includes to access the laplace & normal distributions
(and some std output of course).
*/

#include <boost/math/distributions/laplace.hpp> // for laplace_distribution
  using boost::math::laplace; // typedef provides default type is double.
#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.

#include <iostream>
  using std::cout; using std::endl; using std::left; using std::showpoint; using std::noshowpoint;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;

int main()
{
  cout << "Example: Laplace distribution." << endl;

  try
  {
    { // Traditional tables and values.
/*`Let's start by printing some traditional tables.
*/
      double step = 1.; // in z
      double range = 4; // min and max z = -range to +range.
      //int precision = 17; // traditional tables are only computed to much lower precision.
      int precision = 4; // traditional table at much lower precision.
      int width = 10; // for use with setw.

      // Construct standard laplace & normal distributions l & s
        normal s; // (default location or mean = zero, and scale or standard deviation = unity)
        cout << "Standard normal distribution, mean or location = "<< s.location()
          << ", standard deviation or scale = " << s.scale() << endl;
        laplace l; // (default mean = zero, and standard deviation = unity)
        cout << "Laplace normal distribution, location = "<< l.location()
          << ", scale = " << l.scale() << endl;

/*` First the probability distribution function (pdf).
*/
      cout << "Probability distribution function values" << endl;
      cout << " z  PDF  normal     laplace    (difference)" << endl;
      cout.precision(5);
      for (double z = -range; z < range + step; z += step)
      {
        cout << left << setprecision(3) << setw(6) << z << " "
          << setprecision(precision) << setw(width) << pdf(s, z) << "  "
          << setprecision(precision) << setw(width) << pdf(l, z)<<  "  ("
          << setprecision(precision) << setw(width) << pdf(l, z) - pdf(s, z) // difference.
          << ")" << endl;
      }
      cout.precision(6); // default
/*`Notice how the laplace is less at z = 1 , but has 'fatter' tails at 2 and 3.

   And the area under the normal curve from -[infin] up to z,
   the cumulative distribution function (cdf).
*/
      // For a standard distribution
      cout << "Standard location = "<< s.location()
        << ", scale = " << s.scale() << endl;
      cout << "Integral (area under the curve) from - infinity up to z " << endl;
      cout << " z  CDF  normal     laplace    (difference)" << endl;
      for (double z = -range; z < range + step; z += step)
      {
        cout << left << setprecision(3) << setw(6) << z << " "
          << setprecision(precision) << setw(width) << cdf(s, z) << "  "
          << setprecision(precision) << setw(width) << cdf(l, z) <<  "  ("
          << setprecision(precision) << setw(width) << cdf(l, z) - cdf(s, z) // difference.
          << ")" << endl;
      }
      cout.precision(6); // default

/*`
Pretty-printing a traditional 2-dimensional table is left as an exercise for the student,
but why bother now that the Boost Math Toolkit lets you write
*/
    double z = 2.;
    cout << "Area for gaussian z = " << z << " is " << cdf(s, z) << endl; // to get the area for z.
    cout << "Area for laplace z = " << z << " is " << cdf(l, z) << endl; //
/*`
Correspondingly, we can obtain the traditional 'critical' values for significance levels.
For the 95% confidence level, the significance level usually called alpha,
is 0.05 = 1 - 0.95 (for a one-sided test), so we can write
*/
     cout << "95% of gaussian area has a z below " << quantile(s, 0.95) << endl;
     cout << "95% of laplace area has a z below " << quantile(l, 0.95) << endl;
   // 95% of area has a z below 1.64485
   // 95% of laplace area has a z below 2.30259
/*`and a two-sided test (a comparison between two levels, rather than a one-sided test)

*/
     cout << "95% of gaussian area has a z between " << quantile(s, 0.975)
       << " and " << -quantile(s, 0.975) << endl;
     cout << "95% of laplace area has a z between " << quantile(l, 0.975)
       << " and " << -quantile(l, 0.975) << endl;
   // 95% of area has a z between 1.95996 and -1.95996
   // 95% of laplace area has a z between 2.99573 and -2.99573
/*`Notice how much wider z has to be to enclose 95% of the area.
*/
  }
//] [/[laplace_example1]
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

/*

Output is:

Example: Laplace distribution.
Standard normal distribution, mean or location = 0, standard deviation or scale = 1
Laplace normal distribution, location = 0, scale = 1
Probability distribution function values
 z  PDF  normal     laplace    (difference)
-4     0.0001338   0.009158    (0.009024  )
-3     0.004432    0.02489     (0.02046   )
-2     0.05399     0.06767     (0.01368   )
-1     0.242       0.1839      (-0.05803  )
0      0.3989      0.5         (0.1011    )
1      0.242       0.1839      (-0.05803  )
2      0.05399     0.06767     (0.01368   )
3      0.004432    0.02489     (0.02046   )
4      0.0001338   0.009158    (0.009024  )
Standard location = 0, scale = 1
Integral (area under the curve) from - infinity up to z
 z  CDF  normal     laplace    (difference)
-4     3.167e-005  0.009158    (0.009126  )
-3     0.00135     0.02489     (0.02354   )
-2     0.02275     0.06767     (0.04492   )
-1     0.1587      0.1839      (0.02528   )
0      0.5         0.5         (0         )
1      0.8413      0.8161      (-0.02528  )
2      0.9772      0.9323      (-0.04492  )
3      0.9987      0.9751      (-0.02354  )
4      1           0.9908      (-0.009126 )
Area for gaussian z = 2 is 0.97725
Area for laplace z = 2 is 0.932332
95% of gaussian area has a z below 1.64485
95% of laplace area has a z below 2.30259
95% of gaussian area has a z between 1.95996 and -1.95996
95% of laplace area has a z between 2.99573 and -2.99573

*/


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

//! \brief Compute y = A*x for A banded matrix with diagonal structure
//! \param[in] a n-1 dim. vector for upper diagonal
//! \param[in] b n-2 dim. vector for second lower diagonal
//! \param[in] x n dim. vector for Ax = y
//! \param[out] y n dim. vector y = Ax
template <class Vector>
void multAx(const Vector & a, const Vector & b, const Vector & x, Vector & y) {
    unsigned int n = x.size();
    if( a.size() < n - 1 || b.size() < n - 2 ) {
        // Size check, error if do not match
        std::cerr << "Error: size mismatch!" << std::endl;
        return;
    }
    y.resize(n); // Ensure y has size n

    // Handle first two rows
    if(n > 1) y(0) = 2*x(0) + a(0)*x(1);
    else { // Special case n = 1
        y(0) = 2*x(0);
        return;
    }
    if(n > 2) y(1) = 2*x(1) + a(1)*x(2);
    else { // Special case n = 2
        y(1) = 2*x(1);
        return;
    }

    // Row if n > 3 and without last row
    for(unsigned int i = 2; i < n-1; ++i) {
        y(i) = 2*x(i) + b(i-2)*x(i-2) + a(i)*x(i+1);
    }

    // Last row special case
    if(n > 2) y(n-1) = 2*x(n-1) + b(n-3)*x(n-3);
}

//! \brief Solve y = A*x for A banded matrix with upper tirangular sparse structure
//! \param[in] a n-1 dim. vector for upper diagonal
//! \param[in] r n dim. vector for Ax = r
//! \param[out] x n dim. solution vector r = Ax
template <class Vector>
void solvelseAupper(const Vector & a, const Vector & r, Vector & x) {
    // Dimensions + ensure correct dimensions
    unsigned int n = r.size();
    x.resize(n);

    // Backward substitution
    x(n-1) = 0.5 * r(n-1);
    for(int j = n-2; j >= 0; --j) {
        x(j) = 0.5*(r(j) - a(j)*x(j+1));
    }
}

//! \brief Solve y = A*x for A banded matrix using Gaussian-elimination (no pivot)
//! \param[in] a n-1 dim. vector for upper diagonal
//! \param[in] b n-2 dim. vector for second lower diagonal
//! \param[in] r n dim. vector for Ax = r
//! \param[out] x n dim. solution vector r = Ax
template <class Vector>
void solvelseA(const Vector & a, const Vector & b, const Vector & r, Vector & x) {
    // Setup type, size and copy data
    typedef typename Vector::Scalar Scalar;
    int n = r.size();
    x = r;

    // Fill in matrix, we reserve 5 nnz per row for Gauss fill-in
    Eigen::SparseMatrix<Scalar> A(n,n);
    A.reserve(5);
    for(int i = 0; i < n; ++i) {
        A.insert(i,i) = 2;
        if(i < n-1) A.insert(i,i+1) = a(i);
        if(i >= 2)  A.insert(i,i-2) = b(i-2);
    }
    A.makeCompressed();

    // 1st stage: elimination (in place)
    for(int i = 0; i < n-1; ++i) {
        for(int k = i+1; k < std::min(i+3,n); ++k) {
            Scalar fac = A.coeffRef(k,i)/A.coeffRef(i,i);
            for(int l = i; l < std::min(i+3,n); ++l) {
                A.coeffRef(k,l) -= fac * A.coeffRef(i,l);
            }
            x(k) -= fac * x(i);
        }
    }

    // 2nd stage: backwards substitution (in place)
    x(n-1) /= A.coeffRef(n-1,n-1);
    for(int i = n-2; i >= 0; --i) {
        for(int k = i+1; k < std::min(i+3,n); ++k) {
            x(i) -= x(k)*A.coeffRef(i,k);
        }
        x(i) /= A.coeffRef(i,i);
    }
}

//! \brief Solve y = A*x for A banded matrix using SparseLU
//! \param[in] a n-1 dim. vector for upper diagonal
//! \param[in] b n-2 dim. vector for second lower diagonal
//! \param[in] r n dim. vector for Ax = r
//! \param[out] x n dim. solution vector r = Ax
template <class Vector>
void solvelseAEigen(const Vector & a, const Vector & b, const Vector & r, Vector & x) {
    // Dimensions
    typedef typename Vector::Scalar Scalar;
    unsigned int n = r.size();

    // Fill in matrix + reserve
    Eigen::SparseMatrix<Scalar> A(n,n);
    A.reserve(3);
    for(unsigned int i = 0; i < n; ++i) {
        A.insert(i,i) = 2;
        if(i < n-1) A.insert(i,i+1) = a(i);
        if(i >= 2)  A.insert(i,i-2) = b(i-2);
    }
    A.makeCompressed();

    // Call SparseLU
    Eigen::SparseLU< Eigen::SparseMatrix<Scalar> >   solver;
    solver.analyzePattern(A);
    solver.compute(A);
    x = solver.solve(r);
}

int main() {
    // Random vectors to check correctness
    std::cout << "*** Check correctness of  banded solvers implementations" << std::endl;
    int n = 9;
    Eigen::VectorXd a = Eigen::VectorXd::Random(n-1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n-2);
    Eigen::VectorXd y = Eigen::VectorXd::Random(n);
    Eigen::VectorXd x;

    std::cout << "Original:" << y << std::endl;

    // Compute y = A*inv(A)*y
    solvelseAupper(a,y,x);
    multAx(a,b,x,y);

    b = Eigen::VectorXd::Random(n-2);

    // Should be same as before
    std::cout << "Upper:" << y << std::endl;

    // Compute y = A*inv(A)*y
    solvelseA(a,b,y,x);
    multAx(a,b,x,y);

    // Should be same as before
    std::cout << "Own:" << y << std::endl;

    // Compute y = A*inv(A)*y
    solvelseAEigen(a,b,y,x);
    multAx(a,b,x,y);

    // Should be same as before
    std::cout << "Eigen:" << y << std::endl;

}

Nummerical Methods for CSE/solutions/solution_2/banded_matrix.cpp


#pragma once

#include <utils/debug_utils.hh>

#include <Eigen/Eigenvalues>

namespace math
{

constexpr double DEFAULT_STEP_SIZE = 0.0009765625;

template<int _rows, int _cols>
using Matrix = Eigen::Matrix<double, _rows, _cols>;

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

template<int _rows, int _cols>
bool is_equal(const Matrix<_rows, _cols> &mat1, const Matrix<_rows, _cols> &mat2, const double tol = 1e-8)
{
    return ((mat1 - mat2).array().abs() < tol).all();
}

template<int _rows>
bool is_symmetric(const Matrix<_rows, _rows> &mat, const double tol = 1e-8)
{
    return math::is_equal<_rows,_rows>(mat, mat.transpose(), tol);
}

template<int _rows, typename Callable>
Vector<_rows> gradient(
        const Callable &func,
        const Vector<_rows> &pt, const double delta = DEFAULT_STEP_SIZE)
{
    IS_GREATER(delta, 0);

    const int dim = pt.size();
    IS_GREATER(dim, 0);

    Vector<_rows> gradient(dim);

    for (int i = 0; i < dim; ++i)
    {
        Vector<_rows> pt_positive = pt;
        pt_positive[i] += delta;

        Vector<_rows> pt_negative = pt;
        pt_negative[i] -= delta;

        gradient(i) = (func(pt_positive) - func(pt_negative)) / (2.0 * delta);
    }

    return gradient;
}

template<int _in_rows, int _out_rows, typename Callable>
Matrix<_out_rows, _in_rows> jacobian(
        const Callable &func,
        const Vector<_in_rows> &pt, const double delta = DEFAULT_STEP_SIZE)
{
    IS_GREATER(delta, 0);

    Matrix<_out_rows, _in_rows> jacobian; jacobian.setZero();

    for (int i = 0; i < _in_rows; ++i)
    {
        Vector<_in_rows> pt_positive = pt;
        pt_positive[i] += delta;

        Vector<_in_rows> pt_negative = pt;
        pt_negative[i] -= delta;

        jacobian.col(i) = (func(pt_positive) - func(pt_negative)) / (2.0 * delta);
    }

    return jacobian;
}

template<int _rows, typename Callable>
Matrix<_rows, _rows> hessian(
        const Callable &func,
        const Vector<_rows> &pt, const double delta = DEFAULT_STEP_SIZE)
{
    IS_GREATER(delta, 0);

    // Precompute constants.
    const double two_delta = 2.0*delta;
    const double delta_sq = delta*delta;
    const double ij_eq_div = 12.*delta_sq;
    const double ij_neq_div = 4.*delta_sq;

    Matrix<_rows, _rows> hessian; hessian.setZero();
    // Central difference approximation based on
    // https://v8doc.sas.com/sashtml/ormp/chap5/sect28.htm
    for (int i = 0; i < _rows; ++i)
    {
        for (int j = i; j < _rows; ++j)
        {
            Vector<_rows> p1 = pt;
            Vector<_rows> p2 = pt;
            Vector<_rows> p3 = pt;
            Vector<_rows> p4 = pt;
            double value = 0;
            if (i == j)
            {
                p1[i] += two_delta;
                p2[i] += delta;
                p3[i] -= delta;
                p4[i] -= two_delta;
                value = (-func(p1) + 16.*(func(p2) + func(p3)) - 30.*func(pt) - func(p4))
                    / (ij_eq_div);
            }
            else
            {
                p1[i] += delta;
                p1[j] += delta;

                p2[i] += delta;
                p2[j] -= delta;

                p3[i] -= delta;
                p3[j] += delta;

                p4[i] -= delta;
                p4[j] -= delta;

                value = (func(p1) - func(p2) - func(p3) + func(p4)) / (ij_neq_div);
            }

            hessian(i,j) = value;
            hessian(j,i) = value;
        }
    }

    return hessian;
}

template<int _rows>
Matrix<_rows, _rows> project_to_psd(const Matrix<_rows, _rows> &symmetric_mat, const double min_eigval)
{
    // Cheap check to confirm matrix is square.
    IS_EQUAL(symmetric_mat.rows(), symmetric_mat.cols());

    // O(n^2) check to make sure that the matrix is symmetric.
    IS_TRUE(math::is_symmetric(symmetric_mat));

    const Eigen::SelfAdjointEigenSolver<Matrix<_rows, _rows>> es(symmetric_mat);
    Matrix<_rows, _rows> evals = es.eigenvalues().asDiagonal();
    const Matrix<_rows, _rows> evecs = es.eigenvectors();

    // Indices of eigen values that are less than the minimum eigenvalue threshold.
    int num_failed_evals = 0;
    const int num_evals = evals.rows();
    for (int i = 0; i < num_evals; ++i)
    {
        if (evals(i,i) < min_eigval)
        {
            ++num_failed_evals;
            evals(i,i) = min_eigval;
        }
    }
    // If we didn't have eigenvalues less than the minimum threshold, then return the original
    // matrix.
    if (num_failed_evals == 0)
    {
        return symmetric_mat;
    }

    // We can reconstruct with P D P\inv = P D P^T if P is orthonormal.
    const Matrix<_rows, _rows> projection = evecs * evals * evecs.transpose() ;
    return projection;
}

template<int _rows>
void check_psd(const Matrix<_rows, _rows> &mat, const double min_eigval)
{
    // Cheap check to confirm matrix is square.
    IS_EQUAL(mat.rows(), mat.cols());

    // O(n^2) check to make sure that the matrix is symmetric.
    IS_TRUE(math::is_symmetric(mat));

    const Eigen::SelfAdjointEigenSolver<Matrix<_rows, _rows>> es(mat, Eigen::EigenvaluesOnly);
    const auto evals = es.eigenvalues();

    for (int i = 0; i < evals.size(); ++i)
    {
        // Throw an exception if this is not true.
        IS_GREATER_EQUAL(evals(i), min_eigval);
    }
}

} // namespace math


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

using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 100

int main(int argc, char** argv) {
  MatrixXd matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose(); // 保证实对称矩阵
  SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_NN); // 实对称矩阵可以保证正定成功
  cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
  cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;
  MatrixXd v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

  clock_t time_stt = clock();
  Matrix<double, MATRIX_SIZE, 1> x;

  // QR
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // Cholesky
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  return 0;
}


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

using namespace Eigen;
using namespace std;

int main()
{
  MatrixXf m(2,2);
  m << 1,-2,
       -3,4;

  cout << "1-norm(m)     = " << m.cwiseAbs().colwise().sum().maxCoeff()
       << " == "             << m.colwise().lpNorm<1>().maxCoeff() << endl;

  cout << "infty-norm(m) = " << m.cwiseAbs().rowwise().sum().maxCoeff()
       << " == "             << m.rowwise().lpNorm<1>().maxCoeff() << endl;
}


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

//Transform, Translation, Scaling, Rotation2D and 3D rotations (Quaternion, AngleAxis)

#if KDL_FOUND==1
#include <kdl/frames.hpp>
#endif

//The value of KDL_FOUND has been set via target_compile_definitions in CMake

Eigen::Matrix3d eulerAnglesToRotationMatrix(double roll, double pitch,double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
    Eigen::Matrix3d rotationMatrix = q.matrix();
    return rotationMatrix;
}

void transformation()
{
/*
Great Tutorial:
http://planning.cs.uiuc.edu/node102.html
http://euclideanspace.com/maths/geometry/rotations/conversions/index.htm

Tait–Bryan angles: Z1Y2X3 in the wiki page:
https://en.wikipedia.org/wiki/Euler_angles
  yaw:
      A yaw is a counterclockwise rotation of alpha about the  z-axis. The
  rotation matrix is given by

      R_z

      |cos(alpha) -sin(alpha) 0|
      |sin(apha)   cos(alpha) 0|
      |    0            0     1|

  pitch:
      R_y
      A pitch is a counterclockwise rotation of  beta about the  y-axis. The
  rotation matrix is given by

      |cos(beta)  0   sin(beta)|
      |0          1       0    |
      |-sin(beta) 0   cos(beta)|

  roll:
      A roll is a counterclockwise rotation of  gamma about the  x-axis. The
  rotation matrix is given by
      R_x
      |1          0           0|
      |0 cos(gamma) -sin(gamma)|
      |0 sin(gamma)  cos(gamma)|

      It is important to note that   R_z R_y R_x performs the roll first, then the pitch, and finally the yaw
      Roration matrix: R_z*R_y*R_x

*/
/////////////////////////////////////Rotation Matrix (Tait–Bryan)///////////////////////////////
    double roll, pitch, yaw;
    roll=M_PI/2;
    pitch=M_PI/2;
    yaw=0;//M_PI/6;
    std::cout << "Roll : " <<  roll << std::endl;
    std::cout << "Pitch : " << pitch  << std::endl;
    std::cout << "Yaw : " << yaw  << std::endl;

/////////////////////////////////////Rotation Matrix (Tait–Bryan)///////////////////////////////

    // Roll, Pitch, Yaw to Rotation Matrix
    //Eigen::AngleAxis<double> rollAngle(roll, Eigen::Vector3d(1,0,0));

    std::cout << "Roll : " <<  roll << std::endl;
    std::cout << "Pitch : " << pitch  << std::endl;
    std::cout << "Yaw : " << yaw  << std::endl;

    // Roll, Pitch, Yaw to Rotation Matrix
    //Eigen::AngleAxis<double> rollAngle(roll, Eigen::Vector3d(1,0,0));
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

//////////////////////////////////////// Quaternion ///////////////////////////////////////////
    Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;

    //Quaternion to Rotation Matrix
    Eigen::Matrix3d rotationMatrix = q.matrix();
    std::cout << "3x3 Rotation Matrix" << std::endl;

    std::cout << rotationMatrix << std::endl;

    Eigen::Quaterniond quaternion_mat(rotationMatrix);
    std::cout << "Quaternion X: " << quaternion_mat.x() << std::endl;
    std::cout << "Quaternion Y: " << quaternion_mat.y() << std::endl;
    std::cout << "Quaternion Z: " << quaternion_mat.z() << std::endl;
    std::cout << "Quaternion W: " << quaternion_mat.w() << std::endl;

//////////////////////////////////////// Rodrigues ///////////////////////////////////////////

    //Rotation Matrix to Rodrigues
    Eigen::AngleAxisd rodrigues(rotationMatrix );
    std::cout<<"Rodrigues Angle:\n"<<rodrigues.angle() <<std::endl;

    std::cout<<"Rodrigues Axis:" <<std::endl;

    std::cout<<rodrigues.axis().x() <<std::endl;
    std::cout<<rodrigues.axis().y() <<std::endl;
    std::cout<<rodrigues.axis().z() <<std::endl;

    Eigen::Vector3d vector3d(2.3,3.1,1.7);
    Eigen::Vector3d vector3dNormalized=vector3d.normalized();
    double theta=M_PI/7;
    Eigen::AngleAxisd angleAxisConversion(theta,vector3dNormalized);
    Eigen::Matrix3d rotationMatrixConversion;

    // Angle Axis (Rodrigues) to Rotation Matrix
    rotationMatrixConversion=angleAxisConversion.toRotationMatrix();

    //Rotation Matrix to Quaternion

    Eigen::Quaterniond QuaternionConversion(rotationMatrixConversion);

    //Rotation Matrix to Euler Angle (Proper)
    Eigen::Vector3d euler_angles = rotationMatrixConversion.eulerAngles(2, 0, 2);

    //Eigen::Quaterniond
    Eigen::Quaterniond tmp = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ())
     * Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitX())
     * Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitZ());

////////////////////////////////////////Comparing with KDL////////////////////////////////////////
#if KDL_FOUND==1
    KDL::Frame F;
    F.M = F.M.RPY(roll, pitch, yaw);
    std::cout << F.M(0, 0) << " " << F.M(0, 1) << " " << F.M(0, 2) << std::endl;
    std::cout << F.M(1, 0) << " " << F.M(1, 1) << " " << F.M(1, 2) << std::endl;
    std::cout << F.M(2, 0) << " " << F.M(2, 1) << " " << F.M(2, 2) << std::endl;

    double x, y, z, w;
    F.M.GetQuaternion(x, y, z, w);
    std::cout << "KDL Frame Quaternion:" << std::endl;
    std::cout << "x: " << x << std::endl;
    std::cout << "y: " << y << std::endl;
    std::cout << "z: " << z << std::endl;
    std::cout << "w: " << w << std::endl;
#endif

////////////////////////////////////////Comparing with KDL////////////////////////////////////////

    Eigen::Matrix3d rotation;
    rotation= eulerAnglesToRotationMatrix(roll, pitch,yaw);

    double 	txLeft, tyLeft, tzLeft;
    txLeft=-1;
    tyLeft=0.0;
    tzLeft=-4.0;

    Eigen::Affine3f t1;
    Eigen::Matrix4f M;
    Eigen::Vector3d translation;
    translation<<txLeft,tyLeft,tzLeft;

    M<<  rotation(0,0),rotation(0,1),rotation(0,2),translation(0,0)
     ,rotation(1,0),rotation(1,1),rotation(1,2),translation(1,0)
     ,rotation(2,0),rotation(2,1),rotation(2,2),translation(2,0)
     ,0,0,0,1;

    t1 = M;

    Eigen::Affine3d transform_2 = Eigen::Affine3d::Identity();

    // Define a translation of 2.5 meters on the x axis.
    transform_2.translation() << 2.5, 1.0, 0.5;

    // The same rotation matrix as before; tetha radians arround Z axis
    transform_2.rotate (yawAngle*pitchAngle *rollAngle );
    std::cout<<transform_2.matrix() <<std::endl;
    std::cout<<transform_2.translation()<<std::endl;
    std::cout<<transform_2.translation().x()<<std::endl;
    std::cout<<transform_2.translation().y()<<std::endl;
    std::cout<<transform_2.translation().z()<<std::endl;

}

void determiningRollPitchYawFromRotationMatrix()
{
    /*  http://planning.cs.uiuc.edu/node103.html

      |r11 r12 r13 |
      |r21 r22 r23 |
      |r31 r32 r33 |

      yaw: alpha=arctan(r21/r11)
      pitch: beta=arctan(-r31/sqrt( r32^2+r33^2 ) )
      roll: gamma=arctan(r32/r33)
  */
    double roll, pitch, yaw;
    roll = M_PI / 2;
    pitch = M_PI / 2;
    yaw = 0;
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
    Eigen::Matrix3d rotationMatrix = q.matrix();

    std::cout << "Rotation Matrix is:" << std::endl;
    std::cout << rotationMatrix << std::endl;

    std::cout << "roll is Pi/"
              << M_PI / atan2(rotationMatrix(2, 1), rotationMatrix(2, 2))
              << std::endl;
    std::cout << "pitch: Pi/"
              << M_PI / atan2(-rotationMatrix(2, 0),
                            std::pow(
                                rotationMatrix(2, 1) * rotationMatrix(2, 1) + rotationMatrix(2, 2) * rotationMatrix(2, 2),
                                0.5))
              << std::endl;
    std::cout << "yaw is Pi/"
              << M_PI / atan2(rotationMatrix(1, 0), rotationMatrix(0, 0))
              << std::endl;
}

void transformExample()
{
/*
The class Eigen::Transform  represents either
1) an affine, or
2) a projective transformation
using homogenous calculus.

For instance, an affine transformation A is composed of a linear part L and a translation t such that transforming a point p by A is equivalent to:

p' = L * p + t

Using homogeneous vectors:

    [p'] = [L t] * [p] = A * [p]
    [1 ]   [0 1]   [1]       [1]

Ref: https://stackoverflow.com/questions/35416880/what-does-transformlinear-return-in-the-eigen-library

Difference Between Projective and Affine Transformations
1) The projective transformation does not preserve parallelism, length, and angle. But it still preserves collinearity and incidence.
2) Since the affine transformation is a special case of the projective transformation,
it has the same properties. However unlike projective transformation, it preserves parallelism.

Ref: https://www.graphicsmill.com/docs/gm5/Transformations.htm
*/

    float arrVertices [] = { -1.0 , -1.0 , -1.0 ,
    1.0 , -1.0 , -1.0 ,
    1.0 , 1.0 , -1.0 ,
    -1.0 , 1.0 , -1.0 ,
    -1.0 , -1.0 , 1.0 ,
    1.0 , -1.0 , 1.0 ,
    1.0 , 1.0 , 1.0 ,
    -1.0 , 1.0 , 1.0};
    Eigen::MatrixXf mVertices = Eigen::Map < Eigen::Matrix <float , 3 , 8 > > ( arrVertices ) ;
    Eigen::Transform <float , 3 , Eigen::Affine > t = Eigen::Transform <float , 3 , Eigen::Affine >::Identity();
    t.scale ( 0.8f ) ;
    t.rotate ( Eigen::AngleAxisf (0.25f * M_PI , Eigen::Vector3f::UnitX () ) ) ;
    t.translate ( Eigen::Vector3f (1.5 , 10.2 , -5.1) ) ;
    std::cout << t * mVertices.colwise().homogeneous () << std::endl;
}

Eigen::Matrix4f createAffinematrix(float a, float b, float c, Eigen::Vector3f trans)
{
    {
        Eigen::Transform<float, 3, Eigen::Affine> t;
        t = Eigen::Translation<float, 3>(trans);
        t.rotate(Eigen::AngleAxis<float>(a, Eigen::Vector3f::UnitX()));
        t.rotate(Eigen::AngleAxis<float>(b, Eigen::Vector3f::UnitY()));
        t.rotate(Eigen::AngleAxis<float>(c, Eigen::Vector3f::UnitZ()));
        return t.matrix();
    }

    {
    /*
    The difference between the first implementation and the second is like the difference between "Fix Angle" and "Euler Angle", you can
    https://www.youtube.com/watch?v=09xVHo1JudY
    */
        Eigen::Transform<float, 3, Eigen::Affine> t;
        t = Eigen::AngleAxis<float>(c, Eigen::Vector3f::UnitZ());
        t.prerotate(Eigen::AngleAxis<float>(b, Eigen::Vector3f::UnitY()));
        t.prerotate(Eigen::AngleAxis<float>(a, Eigen::Vector3f::UnitX()));
        t.pretranslate(trans);
        return t.matrix();
    }
}

int main()
{

}


/* Main routine to run a stencil-based finite-difference method for solution of a
   second-order, scalar-valued BVP:

      u'' = p(t)*u' + q(t)*u + r(t),  a<t<b,
      u(a) = ua,  u(b) = ub

   where the problem has stiffness that may be adjusted using
   the real-valued parameter lambda<0 [read from the command line]

   This driver attempts to solve the problem using a second-order, stencil-based
   finite-difference approximation.

   D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

#include <iostream>
#include <iomanip>
#include <vector>
#include <armadillo>
#include "bvp.hpp"

using namespace std;
using namespace arma;

#ifndef USE_SPARSE
#define SPARSE_LINALG 0
#else
#define SPARSE_LINALG 1
#endif

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

  // get lambda from the command line, otherwise set to -10
  double lambda;
  if (argc > 1)
    lambda = atof(argv[1]);
  if (lambda >= 0.0)
    lambda = -10.0;

  // define BVP object
  BVP bvp(lambda);

  // loop over spatial resolutions for tests
  vector<int> N = {100, 1000, 10000};
  for (int i=0; i<N.size(); i++) {

    // output problem information
    cout << "\nStencil-based FD method for BVP with lambda = "
         << lambda << ":" << ",  N = " << N[i] << endl;

    // compute/store analytical solution
    vec t = linspace(bvp.a, bvp.b, N[i]+1);
    double h = t(1)-t(0);
    vec utrue(N[i]+1);
    for (int j=0; j<=N[i]; j++)
      utrue(j) = bvp.utrue(t(j));

    // create matrix and right-hand side vectors
#ifdef SPARSE_LINALG
    sp_mat A(N[i]+1,N[i]+1);
#else
    mat A(N[i]+1,N[i]+1);
    A.fill(0.0);
#endif
    vec b(N[i]+1);
    b.fill(0.0);

    // set up linear system
    A(0,0) = 1.0;
    b(0) = bvp.ua;
    A(N[i],N[i]) = 1.0;
    b(N[i]) = bvp.ub;
    for (int j=1; j<N[i]; j++) {
      A(j,j-1) = -1.0 - 0.5*h*bvp.p(t(j));
      A(j,j) = 2.0 + h*h*bvp.q(t(j));
      A(j,j+1) = -1.0 + 0.5*h*bvp.p(t(j));
      b(j) = -h*h*bvp.r(t(j));
    }

    // solve linear system for BVP solution
#ifdef SPARSE_LINALG
    vec u = arma::spsolve(A,b);
#else
    vec u = arma::solve(A,b);
#endif

    // output maximum error
    vec uerr = abs(u-utrue);
    cout << "  Maximum BVP solution error = " << std::setprecision(4)
       << uerr.max() << "\n";
  }

  return 0;
}


//
// This program is used to study how to calculate (SS|SS)^{m}
// for the recursive relation. Basically, it focus on the
// fmt function:
// f_{m}(t) = \int^{1}_{0} u^{2m} e^{-tu^{2}} du
// it needs the boost library to calcualte fmt
// fenglai liu
// Oct. 2013
//
// for reference, see Harris paper and the reference cited inside:
// Harris, Frank E
// Evaluation of GTO molecular integrals
// International Journal of Quantum Chemistry, 1983, Vol. 23, Page 1469--1478
//
// note:
// we found that the approximation of (2m-1)!!/R expression is
// not numerical stable. Combined with down recursive relation,
// it introduces greater error compared with other methods.
// Therefore we finally abadon it.
//
//
//

// common C head files used in the program
#include<cstdlib>
#include<cstdio>

// external math functions
#include<cmath>

// common C++ head files may be used in the program
#include<iostream>
#include<fstream>
#include<iomanip>
#include<string>
#include<vector>
#include<list>
#include<set>
#include<map>
#include<iterator>
#include<algorithm>

#include <boost/math/special_functions/binomial.hpp>  // special functions in math
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/timer.hpp>
using namespace boost::math;
using namespace boost;
using namespace std;
#define PI              3.1415926535897932384626E0

double fm(const double& a, const int& m);
double doubleFac(const int& m, const int& n);

double fm(const double& a, const int& m) {
	double x = 1.0E0;
	if (fabs(a)<1.0E-14) return (pow(x,2*m+1)/(2*m+1));
	double f12 = 1.0E0/2.0E0;
	double p   = 1.0E0/(2.0E0*pow(a,m+f12));
	double upperLimit = a*pow(x,2.0E0);
	return tgamma_lower(m+f12,upperLimit)*p;
}

double doubleFac(const int& m, const int& n) {
	// calculate (2m-1)!!/(2m+2n+1)!!
	double x = 1.0E0;
	for(int i=0; i<=n; i++) {
		x *= 1.0E0/(2*m+2*i+1);
	}
	return x;
}

int main()
{
	///////////////////////////////////////////////////
	// global settings
	// step length and number of claculation for
	// time performance
	// global error is the error range
	// if the difference is less than the error, we
	// think the error could be omitted
	///////////////////////////////////////////////////
	int N = 10000000;
	double steplength  = 0.000001;
	double globalError = 1.0E-14;
	int top_M_limmit   = 40;
	double T_max_limit = 40.0E0;
	float steplength_f  = 0.00001;
	float globalError_f = 1.0E-6;

	///////////////////////////////////////////////////
	// this section testing the timing performance
	// between the different functions
	// so far, erf is 160 faster than the fmt function
	// in boost library
	///////////////////////////////////////////////////
	cout << "===========================================================" << endl;
	cout << "Time performance result: " << endl;
	cout << "Total sample number is " << N << endl;
	cout << "===========================================================" << endl;
	// testing the erf
	timer t1;
	for(int i=0; i<N; i++) {
		double a = i;
		double x = a/1000;
		double e = erf(x);
	}
	printf("erf time consuming: %-14.7f\n", t1.elapsed());

	// testing the sqrt
	timer t2;
	for(int i=0; i<N; i++) {
		double e = sqrt(i);
	}
	printf("sqrt time consuming: %-14.7f\n", t2.elapsed());

	// testing the exp
	timer t3;
	for(int i=0; i<N; i++) {
		double T = -(i+1)/N;
		double e = exp(T);
	}
	printf("exp(-T) time consuming: %-14.7f\n", t3.elapsed());

	// testing the fmt
	timer t4;
	for(int m=0; m<25; m++) {
		int n = N/25;
		for(int i=0; i<n; i++) {
			double T = 0.0001*(i+1);
			double e = fm(T,m);
		}
	}
	printf("fm(T,m) time consuming: %-14.7f\n", t4.elapsed());

	///////////////////////////////////////////////////
	// testing the fmt from down recursive relation
	///////////////////////////////////////////////////
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to " << top_M_limmit << endl;
	cout << "this is for double type variable" << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from 0 " << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	int nSteps = T_max_limit/steplength;
	for (int n=1; n<=top_M_limmit; n++) {
		for (int j = 0; j<nSteps; j++) {
			double T = steplength*j;
			double fmt= fm(T,n);
			double et = exp(-T);
			double e0 = fmt;
			double t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				double s = fm(T,i);
				double e = (1.0/(2*i+1.0))*(t2*e0+et);
				double d = fabs(s-e);
				if (d>globalError) {
					printf("T is %-16.14f\n", T);
					printf("for m=%d difference is %-16.14f\n", n, d);
				}
				e0 = e;
			}
		}
	}

	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to " << top_M_limmit << endl;
	cout << "this is for float type variable" << endl;
	cout << "error range is " << globalError_f << endl;
	cout << "step length for T is " << steplength_f << endl;
	cout << "T is ranging from 0 " << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	nSteps = T_max_limit/steplength_f;
	for (int n=1; n<=top_M_limmit; n++) {
		for (int j = 0; j<nSteps; j++) {
			float T = steplength_f*j;
			float fmt= fm(T,n);
			float et = exp(-T);
			float e0 = fmt;
			float t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				float s = fm(T,i);
				float e = (1.0/(2*i+1.0))*(t2*e0+et);
				float d = fabs(s-e);
				if (d>globalError_f) {
					printf("T is %-10.7f\n", T);
					printf("for m=%d difference is %-10.7f\n", n, d);
				}
				e0 = e;
			}
		}
	}

	return 0;
}


//  Diagonalizing tridiagonal Toeplitz matrix  with Lapack functions
//  Compile as c++ -O3 -o Tridiag.x TridiagToeplitz.cpp -larmadillo -llapack -lblas
#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <armadillo>
#include "Tridiag.h"
using namespace  std;
using namespace  arma;

vec GetEigenvalues(int Dim)
 {
   int       i, j;
   double    RMin, RMax, Step, DiagConst, NondiagConst;
   RMin = 0.0; RMax = 1.0;
   mat Hamiltonian = zeros<mat>(Dim,Dim);
   // Integration step length
   Step    = RMax/ Dim;
   DiagConst = 2.0 / (Step*Step);
   NondiagConst =  -1.0 / (Step*Step);
   // Setting up tridiagonal matrix and diagonalization using Armadillo
   Hamiltonian(0,0) = DiagConst;
   Hamiltonian(0,1) = NondiagConst;
   for(i = 1; i < Dim-1; i++) {
     Hamiltonian(i,i-1)    = NondiagConst;
     Hamiltonian(i,i)    = DiagConst;
     Hamiltonian(i,i+1)    = NondiagConst;
   }
   Hamiltonian(Dim-1,Dim-2) = NondiagConst;
   Hamiltonian(Dim-1,Dim-1) = DiagConst;
   // diagonalize and obtain eigenvalues
   vec Eigval(Dim);
   eig_sym(Eigval, Hamiltonian);
   return Eigval;
}  //  end of eigenvalue function


#include "lrgGradientDescentSolverStrategy.h"
#include <vector>
#include <random>
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

// I havent used namespaces elsewhere, but eigen reccomended it on its own page, so I used it, std is kept

GradientSolver::GradientSolver(float _eta,int _iterations,float _batch_size)
{
    eta = _eta;
    iterations = _iterations;
    batch_size = _batch_size;
}

// the inputs specified in the header file

std::pair<double, double> &GradientSolver::FitData(std::vector<std::pair<double, double> >&Inputs,std::pair<double, double>&Theta){

// create the eigen matricies needed to do the linear algebra

    VectorXd Y_e = VectorXd::Ones(Inputs.size());
    MatrixXd M_X = MatrixXd::Ones(Inputs.size(),2);

// convert the double pair vector inputs from std to eigen arrays, simply go element by element and copy them over.
// note; the first column should be 1's and the second the actual data for M_X.

    for(int i=0; i < Inputs.size();++i){
        Y_e(i) = Inputs[i].second;
        M_X.col(1)(i) = Inputs[i].first;
    }

    MatrixXd Gradient;
    MatrixXd Theta_e = MatrixXd::Random(2,1);

// the meat of the equation, note that batch_size is a float purely because you cant divide cleanly by integers.

    for(int k=0; k < iterations; ++k){
        Gradient = (2/batch_size) * (M_X.transpose()*((M_X*Theta_e)-Y_e));
        Theta_e = (Theta_e - (eta*Gradient));
    }

    Theta.first = Theta_e(1);
    Theta.second = Theta_e(0);

    return Theta;
};


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

using namespace std;
int main()
{
  Eigen::MatrixXf mat(2,4);
  mat << 1, 2, 6, 9,
         3, 1, 7, 2;

  std::cout << "Column's maximum: " << std::endl
   << mat.colwise().maxCoeff() << std::endl;
}


/**
 * This file is part of https://github.com/adrelino/interpolation-methods
 *
 * Copyright (c) 2018 Adrian Haarbach <mail@adrian-haarbach.de>
 *
 * For the full copyright and license information, please view the LICENSE
 * file that was distributed with this source code.
 */
#ifndef INTERPOL_B_SPLINE_HPP
#define INTERPOL_B_SPLINE_HPP

#include <Eigen/Dense>

namespace interpol {

namespace b_spline {

//important: do not use c++11 auto with Eigen expressions: https://eigen.tuxfamily.org/dox/TopicPitfalls.html

/**
 * Basis form
 */
const Eigen::Matrix4d C_basis = (Eigen::Matrix4d() << \
        -1/6.0,  3/6.0, -3/6.0, 1/6.0,
        3/6.0, -6/6.0,  3/6.0, 0/6.0,
        -3/6.0,  0/6.0,  3/6.0, 0/6.0,
        1/6.0,  4/6.0,  1/6.0, 0/6.0).finished();

template<typename T>
Eigen::Matrix<T, 1, 4> spline_B_basis(double u) {
    return (Eigen::RowVector4d(u*u*u,u*u,u,1)*C_basis).cast<T>();
}

template<typename T>
Eigen::Matrix<T, 1, 4> spline_B_basis_dot(double u) {
    return (Eigen::RowVector4d(3*u*u,2*u,1,0)*C_basis).cast<T>();
}

template<typename T>
Eigen::Matrix<T, 1, 4> spline_B_basis_dotdot(double u) {
    return (Eigen::RowVector4d(6*u,2,0,0)*C_basis).cast<T>();
}

/**
 * Cumulative form
 */
const Eigen::Matrix4d C_cumul = (Eigen::Matrix4d() << \
        6.0 / 6.0 , 0.0       , 0.0        , 0.0        ,
        5.0 / 6.0 , 3.0 / 6.0 , -3.0 / 6.0 , 1.0 / 6.0  ,
        1.0 / 6.0 , 3.0 / 6.0 , 3.0 / 6.0  , -2.0 / 6.0 ,
        0.0       , 0.0       , 0.0        ,  1.0 / 6.0).finished();

template<typename T>
Eigen::Matrix<T, 4, 1> spline_B(double u) {
    return (C_cumul*Eigen::Vector4d(1,u,u*u,u*u*u)).cast<T>();
}

template<typename T>
Eigen::Matrix<T, 4, 1> spline_B_dot(double u, double dt) {
    return (C_cumul*Eigen::Vector4d(0,1/dt,(2*u)/dt,(3*u*u)/dt)).cast<T>();
}

template<typename T>
Eigen::Matrix<T, 4, 1> spline_B_dotdot(double u, double dt) {
    double dt2 = dt * dt;
    return (C_cumul*Eigen::Vector4d(0,0,2/dt2,(6*u)/dt2)).cast<T>();
}

} // ns b_spline

} // ns interpol

#endif //INTERPOL_B_SPLINE_HPP


// factorial.cpp: example program to demonstrate factorials with arbitrary precision number systems
//
// Copyright (C) 2017-2021 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include "common.hpp"
// bring in different number systems
#include <boost/multiprecision/cpp_int.hpp>
#include <universal/number/integer/integer>
#define POSIT_THROW_ARITHMETIC_EXCEPTION 1
#include <universal/number/posit/posit>
// bring in the factorial function
#include <universal/functions/factorial.hpp>

int main()
try {
	using namespace std;
	using namespace boost::multiprecision;
	using namespace sw::function;
	using namespace sw::universal;

	int N = 30;
	int128_t v = factorial<int128_t>(N);
	std::cout << typeid(v).name() << " : \n" << v << std::endl;

	cpp_int u = factorial<cpp_int>(N);
	std::cout << typeid(u).name() << " : \n" << u << std::endl;

	integer<128> w = factorial< integer<128> >(N);
	std::cout << typeid(w).name() << " : \n" << w << std::endl;

	using Posit64 = posit<64, 3>;
	Posit64 p64 = factorial<Posit64>(N);
	cout << typeid(p64).name() << " : \n" << setprecision(40) << p64 << endl;

	using Posit128 = posit<128, 4>;
	Posit128 p128 = factorial<Posit128>(N);
	cout << typeid(p128).name() << " : \n" << setprecision(40) << p128 << endl;

	using Posit256 = posit<256, 4>;
	Posit256 p256 = factorial<Posit256>(N);
	cout << typeid(p256).name() << " : \n" << setprecision(40) << p256 << endl;

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


/**
    Author: Dr. Ing. Ahmad Kamal Nasir
    Email: dringakn@gmail.com
    Description:
    Eigen Geometry module provides two different types of transformations.
    Abstract Transformation:
        Such as rotations(angle and axis, quaternions), translations, scaling.
        These transformations are not represented by matrices, never the less
        we can mix them with matrices and vectors in expressions and convert
        them to the matrices if required.
    Projective/Affine Transformation:
        These are represented by the "Transform" class. These are matrices.
        We can construct the Transform from abstract transformation as follows
            Transform t(AngleAxis(angle,axis))
    Note:
        Eigen default to column major storage.
        Projective transformation is the most general set of transformation.
        Affine transformation is a subset of the projective transformation.
        Euclidian transformation is a subset of the Affine transformation.
        Length, Angles and Areas are preserved in the Euclidian transformation.
        Parallel lines remain parallel in  Affine transformation.
    REFERENCE:
        https://eigen.tuxfamily.org/dox/GettingStarted.html
        https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
        Size can be 2,3,4 for fixed size square matrices or X for dynamic size
 */

#include <iostream>//cout, std
#include <Eigen/Dense>//Matrix
#include <Eigen/Geometry>//Transformation
#include <vector>//vector
#include <random>//default_random_engine, uniform_real_distribution

#define D2R(x) (x*M_PI/180.0)
#define R2D(x) (x*180.0/M_PI)

using namespace std;
using namespace Eigen;

int main() {
	default_random_engine rng;
	uniform_real_distribution<double> dist(-5, 5);

	// Create Transformation object
	Transform<double, 3, Projective> t = Transform<double, 3, Projective>::Identity();
	t.scale(1);
	t.translate(Vector3d(1, 2, 3));
	t.rotate(Quaterniond(AngleAxisd(D2R(30), Vector3d::UnitZ())));
	cout << "Transformation:" << endl;
	cout << "Matrix:" << endl << t.matrix() << endl;
	cout << "Translation:" << endl << t.translation() << endl;
	cout << "Rotation:" << endl << t.rotation().eulerAngles(0, 1, 2)*R2D(1) << endl;

	const int N = 5;
	Matrix<double, 3, N> pc1;

	for (int i = 0; i < N; i++) {
		//Vector3d p1;
		//p1 << dist(rng), dist(rng), dist(rng);
		//pc1.conservativeResize(NoChange, pc1.cols() + 1);
		//pc1.col(pc1.cols() - 1) = p1;
		pc1.col(i) << dist(rng), dist(rng), dist(rng);
	}
	cout << "pc1:" << endl << pc1 << endl << "---" << endl;

	// Extract each column and convert it to a homegenous vector and transform it
	// After the transformation remove the last row
	//MatrixXd pc2 = (t * pc1.colwise().homogeneous()).block(0, 0, 3, N);
	MatrixXd pc2 = t * pc1.colwise().homogeneous();
	pc2.conservativeResize(3, NoChange);
	cout << "pc2:" << endl << pc2 << endl << "---" << endl;

	//Extract the mean vector for both pointclouds
	Vector3d mu1 = pc1.rowwise().mean();
	Vector3d mu2 = pc2.rowwise().mean();
	cout << "mu1:" << endl << mu1 << endl << "---" << endl;
	cout << "mu2:" << endl << mu2 << endl << "---" << endl;

	// Subtract the respective mean from each point in the pointcloud
	pc1.colwise() -= mu1;
	pc2.colwise() -= mu2;
	cout << "pc1:" << endl << pc1 << endl << "---" << endl;
	cout << "pc2:" << endl << pc2 << endl << "---" << endl;

	// Calculate the sum of dot products: Sum(pc1 * pc2^T)
	Matrix3d W = pc1 * pc2.transpose();
	cout << "W:" << endl << W << endl << "---" << endl;

	// Calculate the SVD of the matrix
	JacobiSVD<MatrixXd> svd(W, ComputeThinU | ComputeThinV);
	MatrixXd S = svd.singularValues();
	MatrixXd U = svd.matrixU();
	MatrixXd V = svd.matrixV();
	cout << "SVD:" << endl;
	cout << "Singular Values:" << endl << S << endl;
	cout << "U:" << endl << U << endl;
	cout << "V:" << endl << V << endl;

	// Compute rotation
	// R = U*V^T
	// t = mu_x - R*mu_p

	//Matrix3d R = U * V.transpose();
	//Vector3d T = mu1 - R * mu2;
	Matrix3d R = V * U.transpose();
	Vector3d T = mu2 - R * mu1;
	cout << "R:" << endl << R << endl;
	cout << "det(R):" << endl << R.determinant() << endl;
	cout << "Rotation:" << endl << R.eulerAngles(0, 1, 2)*R2D(1) << endl;
	cout << "Translation:" << endl << T << endl;

	return 0;
}


#include <iostream>
#include <vector>
#include <cmath>
#include <ctime>
#include <boost/random/uniform_real.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

typedef unsigned long long int uint64;
typedef long long int int64;

long double f(double *p) {
  //return sin(p[0]) + cos(p[1]) / sqrt(p[2] + p[3] + p[4] + 1) * pow (p[5] + p[6] + p[7] + p[8] + p[9], 5);
  return sin(p[0]) + cos(p[1]);// / sqrt(p[2] + p[3] + p[4] + 1) * pow (p[5] + p[6] + p[7] + p[8] + p[9], 5);
};

double integrate(std::vector<double> &xlow,
                 std::vector<double> &xhigh, uint64 npd)
{
  if (xlow.size() != xhigh.size()) {
    std::cerr << "unequal number of dimensions in xlow and xhigh\n";
    return NAN;
  }

  boost::mt19937 igen(42u);
  igen.seed(static_cast<unsigned int>(std::time(0)));
  boost::variate_generator<boost::mt19937, boost::uniform_real<> >
                  gen(igen, boost::uniform_real<>(0, 1));
  double *point = new double [xlow.size()];

  double mult = 1.0;
  for(uint64 i = 0; i < xlow.size(); ++i) {
    mult = mult * (xhigh[i] - xlow[i]);
  }

  double sigma = 0;
  for(uint64 i = 0; i < npd * xlow.size(); ++i) {
    for (uint64 j = 0; j < xlow.size(); ++j) {
      point[j] = gen() * (xhigh[j] - xlow[j]) + xlow[j];
      // std::cout << point[j] << " ";
    }
    // std::cout << std::endl;
    sigma += f(point);
    // std::cout << mult * sigma / (npd * xlow.size()) << ",\n";
  }

  delete [] point;
  return mult * sigma / (npd * xlow.size());
}

int main(void)
{
//  std::vector<double> xlow  { 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0};
//  std::vector<double> xhigh { 1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0,  1.0};
  std::vector<double> xlow  { 0.0,  0.0};
  std::vector<double> xhigh { 1.0,  1.0};

  std::cout << "I = " << integrate(xlow, xhigh, 1000000) << std::endl;

  return 0;
};


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

// Define a 2nd order dual type using HigherOrderDual<N> construct.
using dual2nd = HigherOrderDual<2>;

// The scalar function for which the gradient is needed
dual2nd f(const VectorXdual2nd& x)
{
    return x.cwiseProduct(x).sum(); // sum([x(i) * x(i) for i = 1:5])
}

int main()
{
    VectorXdual2nd x(3); // the input vector x with 3 variables
    x << 1, 2, 3; // x = [1, 2, 3]

    dual2nd u; // the output scalar u = f(x) evaluated together with Hessian below
    VectorXdual g;

    MatrixXd H = hessian(f, wrt(x), at(x), u, g); // evaluate the function value u and its Hessian matrix H

    cout << "u = " << u << endl; // print the evaluated output u
    cout << "Hessian = \n" << H << endl; // print the evaluated Hessian matrix H
    cout << "g =\n" << g << endl; // print the evaluated gradient vector gp = [du/dx]
}


#include <iostream>
#include <ctime>
using namespace std;

/* Eigen part */
#include <Eigen/Core>
#include <Eigen/Dense>

#define MATRIX_SIZE 50

int main(int argc, char** argv)
{
    Eigen::Matrix<float, 2, 3> matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();

    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout << matrix_23 << endl;

    for (int i=0; i<1; i++) {
        for (int j=0; j<2; j++) {
            cout << matrix_23(i, j) << endl;
        }
    }

    v_3d << 3, 2, 1;

    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl << endl;

    cout << matrix_33.transpose() << endl;
    cout << matrix_33.sum() << endl;
    cout << matrix_33.trace() << endl;
    cout << matrix_33 * 10 << endl;
    cout << matrix_33.inverse() << endl;
    cout << matrix_33.determinant() << endl;

    cout << matrix_33.inverse() << endl;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "Eigen values = " << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = " << eigen_solver.eigenvectors() << endl;

    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock();

    Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse()*v_Nd;
    cout << "time use in normal inverse is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;

    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in normal inverse is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;

    return 0;
}


//
// This program is used to study how to calculate (SS|SS)^{m}
// for the recursive relation. Basically, it focus on the
// fmt function:
// f_{m}(t) = \int^{1}_{0} u^{2m} e^{-tu^{2}} du
// it needs the boost library to calcualte fmt
// fenglai liu
// Oct. 2013
//
// for reference, see Harris paper and the reference cited inside:
// Harris, Frank E
// Evaluation of GTO molecular integrals
// International Journal of Quantum Chemistry, 1983, Vol. 23, Page 1469--1478
//
// note:
// we found that the approximation of (2m-1)!!/R expression is
// not numerical stable. Combined with down recursive relation,
// it introduces greater error compared with other methods.
// Therefore we finally abadon it.
//
//
//

// common C head files used in the program
#include<cstdlib>
#include<cstdio>

// external math functions
#include<cmath>

// common C++ head files may be used in the program
#include<iostream>
#include<fstream>
#include<iomanip>
#include<string>
#include<vector>
#include<list>
#include<set>
#include<map>
#include<iterator>
#include<algorithm>

#include <boost/math/special_functions/binomial.hpp>  // special functions in math
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/timer.hpp>
using namespace boost::math;
using namespace boost;
using namespace std;
#define PI              3.1415926535897932384626E0

double fm(const double& a, const int& m);
double doubleFac(const int& m, const int& n);

double fm(const double& a, const int& m) {
	double x = 1.0E0;
	if (fabs(a)<1.0E-14) return (pow(x,2*m+1)/(2*m+1));
	double f12 = 1.0E0/2.0E0;
	double p   = 1.0E0/(2.0E0*pow(a,m+f12));
	double upperLimit = a*pow(x,2.0E0);
	return tgamma_lower(m+f12,upperLimit)*p;
}

double doubleFac(const int& m, const int& n) {
	// calculate (2m-1)!!/(2m+2n+1)!!
	double x = 1.0E0;
	for(int i=0; i<=n; i++) {
		x *= 1.0E0/(2*m+2*i+1);
	}
	return x;
}

int main()
{
	///////////////////////////////////////////////////
	// global settings
	// step length and number of claculation for
	// time performance
	// global error is the error range
	// if the difference is less than the error, we
	// think the error could be omitted
	///////////////////////////////////////////////////
	double steplength  = 0.000001;
	double globalError = 1.0E-14;
	int M_limmit       = 8;
	double T_max_limit = 40.0E0;
	double T_min_limit = 1.8E0;
	int numSteps       = (T_max_limit-T_min_limit)/steplength;
	double largest     = 0.0E0;

	/////////////////////////////////////////////////////////////////////
	// this section testing the up recursice relation for M_limit = 8  //
	/////////////////////////////////////////////////////////////////////
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the up recursive relation for m = 1 to " << M_limmit << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from " << T_min_limit << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	for (int j = 0; j<numSteps; j++) {
		double T = T_min_limit+steplength*j;
		double sqT = sqrt(T);
		double fmt= erf(sqT)*sqrt(PI)/2.0E0;
		double et = exp(-T);
		double e0 = fmt/sqT;
		double oneO2T = 0.5E0/T;
		for(int i=1; i<=M_limmit; i++) {
			double s = fm(T,i);
			double e = oneO2T*((2*i-1)*e0-et);
			double d  = fabs(s-e);
			double d0 = s-e;
			if (d>globalError) {
				printf("m is  %d, T   is %-16.14f, difference is %-16.14f\n", i, T, d0);
				if (d>largest) largest = d;
			}
			e0 = e;
		}
	}
	printf("largest difference is %-16.14f\n", largest);

	/////////////////////////////////////////////////////////////////////
	// this section testing the up recursice relation for M_limit = 9  //
	/////////////////////////////////////////////////////////////////////
	cout << endl << endl;
	globalError = 1.0E-13;
	M_limmit    = 9;
	cout << "===========================================================" << endl;
	cout << "testing the up recursive relation for m = 1 to " << M_limmit << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from " << T_min_limit << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	largest = 0.0E0;
	for (int j = 0; j<numSteps; j++) {
		double T = T_min_limit+steplength*j;
		double sqT = sqrt(T);
		double fmt= erf(sqT)*sqrt(PI)/2.0E0;
		double et = exp(-T);
		double e0 = fmt/sqT;
		double oneO2T = 0.5E0/T;
		for(int i=1; i<=M_limmit; i++) {
			double s = fm(T,i);
			double e = oneO2T*((2*i-1)*e0-et);
			double d  = fabs(s-e);
			double d0 = s-e;
			if (d>globalError) {
				printf("m is  %d, T   is %-16.14f, difference is %-16.14f\n", i, T, d0);
				if (d>largest) largest = d;
			}
			e0 = e;
		}
	}
	printf("largest difference is %-16.14f\n", largest);

	/////////////////////////////////////////////////////////////////////
	// this section testing the up recursice relation for M_limit = 10 //
	/////////////////////////////////////////////////////////////////////
	cout << endl << endl;
	globalError = 1.0E-12;
	M_limmit    = 10;
	cout << "===========================================================" << endl;
	cout << "testing the up recursive relation for m = 1 to " << M_limmit << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from " << T_min_limit << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	largest = 0.0E0;
	for (int j = 0; j<numSteps; j++) {
		double T = T_min_limit+steplength*j;
		double sqT = sqrt(T);
		double fmt= erf(sqT)*sqrt(PI)/2.0E0;
		double et = exp(-T);
		double e0 = fmt/sqT;
		double oneO2T = 0.5E0/T;
		for(int i=1; i<=M_limmit; i++) {
			double s = fm(T,i);
			double e = oneO2T*((2*i-1)*e0-et);
			double d  = fabs(s-e);
			double d0 = s-e;
			if (d>globalError) {
				printf("m is  %d, T   is %-16.14f, difference is %-16.14f\n", i, T, d0);
				if (d>largest) largest = d;
			}
			e0 = e;
		}
	}
	printf("largest difference is %-16.14f\n", largest);

	return 0;
}


/* Simple program for solving the two-dimensional diffusion
   equation or Poisson equation using Jacobi's iterative method
   Note that this program does not contain a loop over the time
   dependence. It uses OpenMP to parallelize the spatial part.
*/

#include <iostream>
#include <iomanip>
#include <armadillo>
#include <omp.h>
using namespace std;
using namespace arma;

int JacobiSolver(int, double, double, mat &, mat &, double);

int main(int argc, char * argv[]){
  int Npoints = 100;
  double ExactSolution;
  double dx = 1.0/(Npoints-1);
  double dt = 0.25*dx*dx;
  double tolerance = 1.0e-8;
  mat A = zeros<mat>(Npoints,Npoints);
  mat q = zeros<mat>(Npoints,Npoints);

  int thread_num;
  omp_set_num_threads(4);
  thread_num = omp_get_max_threads ();
  cout << "  The number of processors available = " << omp_get_num_procs () << endl ;
  cout << "  The number of threads available    = " << thread_num <<  endl;
  // setting up an additional source term
  for(int i = 0; i < Npoints; i++)
    for(int j = 0; j < Npoints; j++)
      q(i,j) = -2.0*M_PI*M_PI*sin(M_PI*dx*i)*sin(M_PI*dx*j);

  int itcount = JacobiSolver(Npoints,dx,dt,A,q,tolerance);

  // Testing against exact solution
  double sum = 0.0;
  for(int i = 0; i < Npoints; i++){
    for(int j=0;j < Npoints; j++){
      ExactSolution = -sin(M_PI*dx*i)*sin(M_PI*dx*j);
      sum += fabs((A(i,j) - ExactSolution));
    }
  }
  cout << setprecision(5) << setiosflags(ios::scientific);
  cout << "Jacobi error is " << sum/Npoints << " in " << itcount << " iterations" << endl;
}

// Function for setting up the iterative Jacobi solver
int JacobiSolver(int N, double dx, double dt, mat &A, mat &q, double abstol)
{
  int MaxIterations = 100000;

  double D = dt/(dx*dx);
  // initial guess
  mat Aold = randu<mat>(N,N);
  // Boundary conditions, all zeros
  for(int i=0; i < N; i++){
    A(0,i) = 0.0;
    A(N-1,i) = 0.0;
    A(i,0) = 0.0;
    A(i,N-1) = 0.0;
  }
  double sum = 1.0;
  int k = 0;
  // Start the iterative solver
  while (k < MaxIterations && sum > abstol){
  int i, j;
  sum = 0.0;
  // Define parallel region
# pragma omp parallel default(shared) private (i, j) reduction(+:sum)
  {
     # pragma omp for
    for(i = 1; i < N-1; i++){
      for(j = 1; j < N-1; j++){
	A(i,j) = dt*q(i,j) + Aold(i,j) +
	  D*(Aold(i+1,j) + Aold(i,j+1) - 4.0*Aold(i,j) +
	     Aold(i-1,j) + Aold(i,j-1));
      }
    }
    for(i = 0; i < N;i++){
      for(j = 0; j < N;j++){
	sum += fabs(Aold(i,j)-A(i,j));
	Aold(i,j) = A(i,j);
      }
    }
    sum /= (N*N);
  } //end parallel region
  k++;
  } //end while loop
  return k;
}


// translated from DFT Python code as shown in
// https://www.youtube.com/watch?v=sX-DNi_SX-Q
//
// Compile:
// g++ -g --std=c++11 -o dft dft.cpp  -Ieigen
//
// Execute:
// ./dft  0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
//
// Plot using gnuplot:
// plot "plot_out.txt" using 1:2 with lines lw 3, "plot_in.txt" using 1:2:(0.03) with circles fill solid

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

using std::cout;
using namespace Eigen;
using std::pow;
using std::exp;
using std::ofstream;
using std::ostream;

typedef std::complex<double> complex_t;

complex_t omega( int n )
{
    double arg = 2. * M_PI / n;
    return complex_t( cos(arg), sin(arg));
}

MatrixXcd omegaMatrix( int n )
{
    complex_t om = omega(n);
    MatrixXcd m;
    m.resize( n, n);

    int j,k;
    for( j = 0; j < n; j++)
        for( k = 0; k < n; k++)
            m(j,k) = pow( om, -k * j );

    return m / n;
}

VectorXcd DFT( VectorXcd Y )
{
    int n = Y.size();
    int start = (int)ceil( ((double)n) / 2. ) - 1;
    complex_t oms = pow( omega(n), (double)start);

    for( int k = 0; k < n; k++)
        Y[k] *= pow( oms, (double)k);

    VectorXcd res;
    res = omegaMatrix( n ) * Y;

    for( int k = 0; k < res.size(); k++)
        res[k] *= pow( -1., k + start);

    return res;
}

void print_input( const VectorXcd &Y, ostream &os)
{
    int n = Y.size();

    for( int k = 0; k < n; k++)
        os << (-M_PI + 2. * k * M_PI / n) << "  " << Y[k].real() << "\n";
}

void print_DFTPoly( const VectorXcd &Y, double step_x, ostream &os)
{
    int n = Y.size();

    VectorXcd coeffs = DFT( Y );

    int zero = (n - 1) / 2;

    for( double x = -M_PI; x <= M_PI; x += step_x)
    {
        double S = coeffs[zero].real();
        for( int k = 1; k < (int)ceil( ((double)n) / 2.); k++)
        {
            S += cos(k * x) * (coeffs[zero-k] + coeffs[zero+k]).real();
            S += sin(k * x) * (coeffs[zero-k] - coeffs[zero+k]).imag();
        }

        if( n % 2 == 0 )
            S += cos(n * x / 2.) * coeffs[coeffs.size()-1].real();

        os << x << "  " << S << "\n";
    }
}

int main( int argc, char **argv)
{
    VectorXcd in;

    int n = argc - 1;

    if( n < 3 )
    {
        std::cerr << "input too small\n";
        return 1;
    }

    in.resize(n);

    for( int i = 0; i < n; i++)
        in(i) = atof( argv[i+1] );

    ofstream plot_in( "plot_in.txt" );
    ofstream plot_out( "plot_out.txt" );
    print_input( in, plot_in);
    print_DFTPoly( in, 0.01, plot_out);

    return 0;
}


#include "renderer_math_cpu.h"
#include <Eigen/Eigenvalues>
#include <algorithm>

Eigen::MatrixXd RendererMathCpu::createCompanionMatrix(
	const Eigen::VectorXd& coefficients)
{
	Eigen::Index N = coefficients.size() - 1;
	Eigen::MatrixXd m = Eigen::MatrixXd::Zero(N, N);

	//diagonal, 1 below diagonal
	for (Eigen::Index i = 0; i < N - 1; ++i)
		m(i + 1, i) = 1;

	//add coefficients in the first row
	//scaled by 1/c[0], the highest degree
	for (Eigen::Index i = 0; i < N; ++i)
		m(0, i) = -coefficients[i + 1] / coefficients[0];

	return m;
}

Eigen::VectorXcd RendererMathCpu::rootsFromCompanionMatrix(const Eigen::MatrixXd& m)
{
	Eigen::EigenSolver<Eigen::MatrixXd> es(m, false);
	Eigen::VectorXcd eigenvalues = es.eigenvalues();
	return eigenvalues;
}

std::vector<double> RendererMathCpu::extractRealRoots(const Eigen::VectorXcd& complexRoots, double epsilon)
{
	std::vector<double> realRoots;
	realRoots.reserve(complexRoots.size());
	for (Eigen::Index i=0; i<complexRoots.size(); ++i)
	{
		if (std::abs(complexRoots[i].imag()) < epsilon)
			realRoots.push_back(complexRoots[i].real());
	}
	std::sort(realRoots.begin(), realRoots.end());
	return realRoots;
}


// students_t_example3.cpp
// Copyright Paul A. Bristow 2006, 2007.

// 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)

// Example 3 of using Student's t.

// A general guide to Student's t is at
// http://en.wikipedia.org/wiki/Student's_t-test
// (and many other elementary and advanced statistics texts).
// It says:
// The t statistic was invented by William Sealy Gosset
// for cheaply monitoring the quality of beer brews.
// "Student" was his pen name.
// Gosset was statistician for Guinness brewery in Dublin, Ireland,
// hired due to Claude Guinness's innovative policy of recruiting the
// best graduates from Oxford and Cambridge for applying biochemistry
// and statistics to Guinness's industrial processes.
// Gosset published the t test in Biometrika in 1908,
// but was forced to use a pen name by his employer who regarded the fact
// that they were using statistics as a trade secret.
// In fact, Gosset's identity was unknown not only to fellow statisticians
// but to his employer - the company insisted on the pseudonym
// so that it could turn a blind eye to the breach of its rules.

// The Students't distribution function is described at
// http://en.wikipedia.org/wiki/Student%27s_t_distribution

#include <boost/math/distributions/students_t.hpp>
   using boost::math::students_t;  // Probability of students_t(df, t).

#include <iostream>
   using std::cout;    using std::endl;
#include <iomanip>
   using std::setprecision;    using std::setw;
#include <cmath>
   using std::sqrt;

// This example of a two-sided test is from:
// B. M. Smith & M. B. Griffiths, Analyst, 1982, 107, 253,
// from Statistics for Analytical Chemistry, 3rd ed. (1994), pp 58-59
// J. C. Miller and J. N. Miller, Ellis Horwood ISBN 0 13 0309907

// Concentrations of lead (ug/l) determined by two different methods
// for each of four test portions,
// the concentration of each portion is significantly different,
// the values may NOT be pooled.
// (Called a 'paired test' by Miller and Miller
// because each portion analysed has a different concentration.)

// Portion  Wet oxidation Direct Extraction
//   1           71            76
//   2           61            68
//   3           50            48
//   4           60            57

const int portions = 4;
const int methods = 2;
float data [portions][methods] = {{71, 76}, {61,68}, {50, 48}, {60, 57}};
float diffs[portions];

int main()
{
   cout << "Example3 using Student's t function. " << endl;
   float mean_diff = 0.f;
   cout << "\n""Portion  wet_oxidation  Direct_extraction  difference" << endl;
   for (int portion = 0; portion < portions; portion++)
   { // Echo data and differences.
      diffs[portion] = data[portion][0] - data[portion][1];
      mean_diff += diffs[portion];
      cout << setw(4) << portion << ' ' << setw(14) << data[portion][0] << ' ' << setw(18)<< data[portion][1] << ' ' << setw(9) << diffs[portion] << endl;
   }
   mean_diff /= portions;
   cout << "Mean difference = " << mean_diff << endl; // -1.75

   float sd_diffs = 0.f;
   for (int portion = 0; portion < portions; portion++)
   { // Calculate standard deviation of differences.
      sd_diffs +=(diffs[portion] - mean_diff) * (diffs[portion] - mean_diff);
   }
   int degrees_of_freedom = portions-1; // Use the n-1 formula.
   sd_diffs /= degrees_of_freedom;
   sd_diffs = sqrt(sd_diffs);
   cout << "Standard deviation of differences = " << sd_diffs << endl; // 4.99166
   // Standard deviation of differences = 4.99166
   double t = mean_diff * sqrt(static_cast<double>(portions))/ sd_diffs; // -0.70117
   cout << "Student's t = " << t << ", if " << degrees_of_freedom << " degrees of freedom." << endl;
   // Student's t = -0.70117, if 3 degrees of freedom.
   cout << "Probability of the means being different is "
    << 2.F * cdf(students_t(degrees_of_freedom), t) << "."<< endl; // 0.266846 * 2 = 0.533692
   // Double the probability because using a 'two-sided test' because
   // mean for 'Wet oxidation' could be either
   // greater OR LESS THAN for 'Direct extraction'.

   return 0;
}  // int main()

/*

Output is:

Example3 using Student's t function.
Portion  wet_oxidation  Direct_extraction  difference
   0             71                 76        -5
   1             61                 68        -7
   2             50                 48         2
   3             60                 57         3
Mean difference = -1.75
Standard deviation of differences = 4.99166
Student's t = -0.70117, if 3 degrees of freedom.
Probability of the means being different is 0.533692.

*/