//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/image/image2d.hpp>
#include <boost/compute/interop/opencv/core.hpp>
#include <boost/compute/interop/opencv/highgui.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/uniform_real_distribution.hpp>
#include <boost/compute/utility/dim.hpp>
#include <boost/compute/utility/source.hpp>

namespace compute = boost::compute;

using compute::dim;
using compute::int_;
using compute::float_;
using compute::float2_;

// the k-means example implements the k-means clustering algorithm
int main()
{
    // number of clusters
    size_t k = 6;

    // number of points
    size_t n_points = 4500;

    // height and width of image
    size_t height = 800;
    size_t width = 800;

    // get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);

    // generate random, uniformily-distributed points
    compute::default_random_engine random_engine(queue);
    compute::uniform_real_distribution<float_> uniform_distribution(0, 800);

    compute::vector<float2_> points(n_points, context);
    uniform_distribution.generate(
        compute::make_buffer_iterator<float_>(points.get_buffer(), 0),
        compute::make_buffer_iterator<float_>(points.get_buffer(), n_points * 2),
        random_engine,
        queue
    );

    // initialize all points to cluster 0
    compute::vector<int_> clusters(n_points, context);
    compute::fill(clusters.begin(), clusters.end(), 0, queue);

    // create initial means with the first k points
    compute::vector<float2_> means(k, context);
    compute::copy_n(points.begin(), k, means.begin(), queue);

    // k-means clustering program source
    const char k_means_source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        __kernel void assign_clusters(__global const float2 *points,
                                      __global const float2 *means,
                                      const int k,
                                      __global int *clusters)
        {
            const uint gid = get_global_id(0);

            const float2 point = points[gid];

            // find the closest cluster
            float current_distance = 0;
            int closest_cluster = -1;

            // find closest cluster mean to the point
            for(int i = 0; i < k; i++){
                const float2 mean = means[i];

                int distance_to_mean = distance(point, mean);
                if(closest_cluster == -1 || distance_to_mean < current_distance){
                    current_distance = distance_to_mean;
                    closest_cluster = i;
                }
            }

            // write new cluster
            clusters[gid] = closest_cluster;
        }

        __kernel void update_means(__global const float2 *points,
                                   const uint n_points,
                                   __global float2 *means,
                                   __global const int *clusters)
        {
            const uint k = get_global_id(0);

            float2 sum = { 0, 0 };
            float count = 0;
            for(uint i = 0; i < n_points; i++){
                if(clusters[i] == k){
                    sum += points[i];
                    count += 1;
                }
            }

            means[k] = sum / count;
        }
    );

    // build the k-means program
    compute::program k_means_program =
        compute::program::build_with_source(k_means_source, context);

    // setup the k-means kernels
    compute::kernel assign_clusters_kernel(k_means_program, "assign_clusters");
    assign_clusters_kernel.set_arg(0, points);
    assign_clusters_kernel.set_arg(1, means);
    assign_clusters_kernel.set_arg(2, int_(k));
    assign_clusters_kernel.set_arg(3, clusters);

    compute::kernel update_means_kernel(k_means_program, "update_means");
    update_means_kernel.set_arg(0, points);
    update_means_kernel.set_arg(1, int_(n_points));
    update_means_kernel.set_arg(2, means);
    update_means_kernel.set_arg(3, clusters);

    // run the k-means algorithm
    for(int iteration = 0; iteration < 25; iteration++){
        queue.enqueue_1d_range_kernel(assign_clusters_kernel, 0, n_points, 0);
        queue.enqueue_1d_range_kernel(update_means_kernel, 0, k, 0);
    }

    // create output image
    compute::image2d image(
        context, width, height, compute::image_format(CL_RGBA, CL_UNSIGNED_INT8)
    );

    // program with two kernels, one to fill the image with white, and then
    // one the draw to points calculated in coordinates on the image
    const char draw_walk_source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        __kernel void draw_points(__global const float2 *points,
                                  __global const int *clusters,
                                  __write_only image2d_t image)
        {
            const uint i = get_global_id(0);
            const float2 coord = points[i];

            // map cluster number to color
            uint4 color = { 0, 0, 0, 0 };
            switch(clusters[i]){
              case 0:
                  color = (uint4)(255, 0, 0, 255);
                  break;
              case 1:
                  color = (uint4)(0, 255, 0, 255);
                  break;
              case 2:
                  color = (uint4)(0, 0, 255, 255);
                  break;
              case 3:
                  color = (uint4)(255, 255, 0, 255);
                  break;
              case 4:
                  color = (uint4)(255, 0, 255, 255);
                  break;
              case 5:
                  color = (uint4)(0, 255, 255, 255);
                  break;
            }

            // draw a 3x3 pixel point
            for(int x = -1; x <= 1; x++){
                for(int y = -1; y <= 1; y++){
                    if(coord.x + x > 0 && coord.x + x < get_image_width(image) &&
                       coord.y + y > 0 && coord.y + y < get_image_height(image)){
                        write_imageui(image, (int2)(coord.x, coord.y) + (int2)(x, y), color);
                    }
                }
            }
        }

        __kernel void fill_gray(__write_only image2d_t image)
        {
            const int2 coord = { get_global_id(0), get_global_id(1) };

            if(coord.x < get_image_width(image) && coord.y < get_image_height(image)){
                uint4 gray = { 15, 15, 15, 15 };
                write_imageui(image, coord, gray);
            }
        }
    );

    // build the program
    compute::program draw_program =
        compute::program::build_with_source(draw_walk_source, context);

    // fill image with dark gray
    compute::kernel fill_kernel(draw_program, "fill_gray");
    fill_kernel.set_arg(0, image);

    queue.enqueue_nd_range_kernel(
        fill_kernel, dim(0, 0), dim(width, height), dim(1, 1)
    );

    // draw points colored according to cluster
    compute::kernel draw_kernel(draw_program, "draw_points");
    draw_kernel.set_arg(0, points);
    draw_kernel.set_arg(1, clusters);
    draw_kernel.set_arg(2, image);
    queue.enqueue_1d_range_kernel(draw_kernel, 0, n_points, 0);

    // show image
    compute::opencv_imshow("k-means", image, queue);

    // wait and return
    cv::waitKey(0);

    return 0;
}


//  (C) Copyright Nick Thompson 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#ifndef BOOST_MATH_TOOLS_CUBIC_ROOTS_HPP
#define BOOST_MATH_TOOLS_CUBIC_ROOTS_HPP
#include <algorithm>
#include <array>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/tools/roots.hpp>

namespace boost::math::tools {

// Solves ax^3 + bx^2 + cx + d = 0.
// Only returns the real roots, as types get weird for real coefficients and
// complex roots. Follows Numerical Recipes, Chapter 5, section 6. NB: A better
// algorithm apparently exists: Algorithm 954: An Accurate and Efficient Cubic
// and Quartic Equation Solver for Physical Applications However, I don't have
// access to that paper!
template <typename Real>
std::array<Real, 3> cubic_roots(Real a, Real b, Real c, Real d) {
    using std::abs;
    using std::acos;
    using std::cbrt;
    using std::cos;
    using std::fma;
    using std::sqrt;
    std::array<Real, 3> roots = {std::numeric_limits<Real>::quiet_NaN(),
                                 std::numeric_limits<Real>::quiet_NaN(),
                                 std::numeric_limits<Real>::quiet_NaN()};
    if (a == 0) {
        // bx^2 + cx + d = 0:
        if (b == 0) {
            // cx + d = 0:
            if (c == 0) {
                if (d != 0) {
                    // No solutions:
                    return roots;
                }
                roots[0] = 0;
                roots[1] = 0;
                roots[2] = 0;
                return roots;
            }
            roots[0] = -d / c;
            return roots;
        }
        auto [x0, x1] = quadratic_roots(b, c, d);
        roots[0] = x0;
        roots[1] = x1;
        return roots;
    }
    if (d == 0) {
        auto [x0, x1] = quadratic_roots(a, b, c);
        roots[0] = x0;
        roots[1] = x1;
        roots[2] = 0;
        std::sort(roots.begin(), roots.end());
        return roots;
    }
    Real p = b / a;
    Real q = c / a;
    Real r = d / a;
    Real Q = (p * p - 3 * q) / 9;
    Real R = (2 * p * p * p - 9 * p * q + 27 * r) / 54;
    if (R * R < Q * Q * Q) {
        Real rtQ = sqrt(Q);
        Real theta = acos(R / (Q * rtQ)) / 3;
        Real st = sin(theta);
        Real ct = cos(theta);
        roots[0] = -2 * rtQ * ct - p / 3;
        roots[1] = -rtQ * (-ct + sqrt(Real(3)) * st) - p / 3;
        roots[2] = rtQ * (ct + sqrt(Real(3)) * st) - p / 3;
    } else {
        // In Numerical Recipes, Chapter 5, Section 6, it is claimed that we
        // only have one real root if R^2 >= Q^3. But this isn't true; we can
        // even see this from equation 5.6.18. The condition for having three
        // real roots is that A = B. It *is* the case that if we're in this
        // branch, and we have 3 real roots, two are a double root. Take
        // (x+1)^2(x-2) = x^3 - 3x -2 as an example. This clearly has a double
        // root at x = -1, and it gets sent into this branch.
        Real arg = R * R - Q * Q * Q;
        Real A = (R >= 0 ? -1 : 1) * cbrt(abs(R) + sqrt(arg));
        Real B = 0;
        if (A != 0) {
            B = Q / A;
        }
        roots[0] = A + B - p / 3;
        // Yes, we're comparing floats for equality:
        // Any perturbation pushes the roots into the complex plane; out of the
        // bailiwick of this routine.
        if (A == B || arg == 0) {
            roots[1] = -A - p / 3;
            roots[2] = -A - p / 3;
        }
    }
    // Root polishing:
    for (auto &r : roots) {
        // Horner's method.
        // Here I'll take John Gustaffson's opinion that the fma is a *distinct*
        // operation from a*x +b: Make sure to compile these fmas into a single
        // instruction and not a function call! (I'm looking at you Windows.)
        Real f = fma(a, r, b);
        f = fma(f, r, c);
        f = fma(f, r, d);
        Real df = fma(3 * a, r, 2 * b);
        df = fma(df, r, c);
        if (df != 0) {
            Real d2f = fma(6 * a, r, 2 * b);
            Real denom = 2 * df * df - f * d2f;
            if (denom != 0) {
                r -= 2 * f * df / denom;
            } else {
                r -= f / df;
            }
        }
    }
    std::sort(roots.begin(), roots.end());
    return roots;
}

// Computes the empirical residual p(r) (first element) and expected residual
// eps*|rp'(r)| (second element) for a root. Recall that for a numerically
// computed root r satisfying r = r_0(1+eps) of a function p, |p(r)| <=
// eps|rp'(r)|.
template <typename Real>
std::array<Real, 2> cubic_root_residual(Real a, Real b, Real c, Real d,
                                        Real root) {
    using std::abs;
    using std::fma;
    std::array<Real, 2> out;
    Real residual = fma(a, root, b);
    residual = fma(residual, root, c);
    residual = fma(residual, root, d);

    out[0] = residual;

    // The expected residual is:
    // eps*[4|ar^3| + 3|br^2| + 2|cr| + |d|]
    // This can be demonstrated by assuming the coefficients and the root are
    // perturbed according to the rounding model of floating point arithmetic,
    // and then working through the inequalities.
    root = abs(root);
    Real expected_residual = fma(4 * abs(a), root, 3 * abs(b));
    expected_residual = fma(expected_residual, root, 2 * abs(c));
    expected_residual = fma(expected_residual, root, abs(d));
    out[1] = expected_residual * std::numeric_limits<Real>::epsilon();
    return out;
}

// Computes the condition number of rootfinding. This is defined in Corless, A
// Graduate Introduction to Numerical Methods, Section 3.2.1.
template <typename Real>
Real cubic_root_condition_number(Real a, Real b, Real c, Real d, Real root) {
    using std::abs;
    using std::fma;
    // There are *absolute* condition numbers that can be defined when r = 0;
    // but they basically reduce to the residual computed above.
    if (root == static_cast<Real>(0)) {
        return std::numeric_limits<Real>::infinity();
    }

    Real numerator = fma(abs(a), abs(root), abs(b));
    numerator = fma(numerator, abs(root), abs(c));
    numerator = fma(numerator, abs(root), abs(d));
    Real denominator = fma(3 * a, root, 2 * b);
    denominator = fma(denominator, root, c);
    if (denominator == static_cast<Real>(0)) {
        return std::numeric_limits<Real>::infinity();
    }
    denominator *= root;
    return numerator / abs(denominator);
}

} // namespace boost::math::tools
#endif


/*
 * ConvecMatrixEquationSolverQP.hpp
 *
 *  Created on: August 20, 2020
 *      Author: Quincy Jones
 *
 * Copyright (c) <2020> <Quincy Jones - quincy@implementedrobotics.com/>
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software
 * is furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef NOMAD_CORE_OPTIMALCONTROL_CONVEXMATRIXEQUATIONSOLVERQP_H_
#define NOMAD_CORE_OPTIMALCONTROL_CONVEXMATRIXEQUATIONSOLVERQP_H_

// C System Files

// C++ System Files
#include <iostream>

// Third Party Includes
#include <Eigen/Dense>
#include <qpOASES.hpp>

// Project Includes
#include <Common/Math/MathUtils.hpp>

// Helper class to solve a system of linear equations of the form: A * x = b
// In general you would use this is there is reduncancy in the system (Number of equations/Number of unknowns)
// Therefore this is only solved in a somewhat "least squares sense" by exploiting variable weighting and inequality constraints to handle
// the reduncancy in the sysytem.  This is good because the redundancy in the system allows you to meet secondary objectives.

// x* = min(Ax-b)^T * S * (A*x-b) + alpha*x^T*W1*x + beta*(x-x_prev)^T * W2 * (x-x_prev))
// s.t. lb < C*x < ub

// Maps to a standard QP Formulation using qpOases:
//
// min x      1/2*x^T*H*x + x^t*g
// s.t. lb < A_qp*x > ub // Inequality Contraints

// x* = optimal solution (Num Equations)
// S  = Relative priority of decision variables (Num Equations x Num Equations)
// alpha = decision variable minimization weight (scalar)
// W1   = Relative priority of weight minimization on decision variables (Num Variables x Num Variables)
// beta = solution filtering from a previous solution.  Helps with smoothing from a previous solution (scalar)
// W2   = Relative priority of weight smoothing on decision variables (Num Variables x Num Variables)
// C    = Constraint matrix for solution (Num inequality constraints x Num Variables)

namespace Core::OptimalControl
{
    class ConvexLinearSystemSolverQP
    {

    public:
        // Base Class ConvexLinearSystemSolverQP
        // num_eq = Number of equations of OCP
        // num_vars = Number of veriables of OCP
        ConvexLinearSystemSolverQP(const unsigned int num_eq, const unsigned int num_vars, const unsigned int num_constraints);

        // Solve
        virtual void Solve();

        // Return Current Solution X
        Eigen::VectorXd X() const { return x_star_; }

        double GetSolverTime() const { return solver_time_; }

        void EnableQPDebug(bool enable);
        void PrintDebug();

    protected:

        qpOASES::SQProblem qp_;        // qpOases Solver Object

        qpOASES::int_t max_iterations_;    // Max Iterations for Solver
        qpOASES::int_t solver_iterations_; // Total number of Solver iterations for solution

        Eigen::MatrixXd A_;           // Coefficient Matrix
        Eigen::VectorXd x_star_;      // Solution Vector
        Eigen::VectorXd x_star_prev_; // Previous Solutions Vector
        Eigen::VectorXd b_;           // Constant Vector

        Eigen::MatrixXd S_;           // Relative Priority Weighting of Values
        Eigen::MatrixXd W_1_;         // Relative Priority Weighting of Solution Minimization
        Eigen::MatrixXd W_2_;         // Relative Priority Weighting of Solution Filtering Minimization
        //Eigen::MatrixXd C_;         // Inequality Constraint Matrix

        Eigen::VectorXd lbA_; // Lower Bound of Inequality Constraint Matrix
        Eigen::VectorXd ubA_; // Upper Bound of Inequality Constraint Matrix

        Eigen::MatrixXd H_qp_; // QP Hessian
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A_qp_; // Inequality Constraint Matrix QP
        Eigen::VectorXd g_qp_; // Linear

        double alpha_; // Influence of Solution Minimization
        double beta_;  // Influece of Solution Filtering

        int num_equations_;   // Number of System Equations
        int num_variables_;   // Number of System Variables
        int num_constraints_; // Number of Inequality Constraints

        double solver_time_; // Total time for Solver to compute a solution

        bool solved_; // Valid Solution?

        bool is_hot_; // We have a warm QP and can use hotstarting

        // TODO:
        // Infeasible, BlahBlah
    };
} // namespace Core::OptimalControl

#endif // NOMAD_CORE_OPTIMALCONTROL_CONVEXMATRIXEQUATIONSOLVERQP_H_


#include <string>
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <iostream>
#include <fstream>
#include <cassert>
#include <memory>

#include <Eigen/Dense>
#include "Components.h"
#include "Network.h"
// #include "spdlog/spdlog.h"

using Eigen::MatrixXd;

int main(int argc, char const *argv[])
{
    if (argc != 2){
        std::cout << "Incorrect usage" << std::endl;
        return -1;
    }
    // spdlog::info("Welcome to spdlog!");
    std::ifstream net_file (argv[1]);
    if (!net_file.is_open()){
        std::cout << "Failed to open in file" << std::endl;
        return -1;
    }
    Network net = Network();
    net.preprocess_netlist(net_file);
    net.parse_netlist(net_file);
    net.calculate();

	std::cout << "Conductance\n" << net.get_conductance_matrix() << '\n';
	std::cout << "Incidence\n" << net.get_incidence_matrix() << '\n';
	std::cout << "z\n" << net.get_z_matrix() << '\n';
	std::cout << "result\n" << net.get_result_matrix() << '\n';
	net_file.close();
    return 0;
}

// <comp type><name> <+> <-> <value>
// https://lpsa.swarthmore.edu/Systems/Electrical/mna/MNA3.html#Notational_Convention
/*todo:
	- look at initializing node 0 to id -1, gets rid of us having to use offset
*/


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@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/.
#include "bbw.h"

#include <igl/cotmatrix.h>
#include <igl/massmatrix.h>
#include <igl/invert_diag.h>
#include <igl/speye.h>
#include <igl/slice_into.h>
#include <igl/min_quad_with_fixed.h>

#include <Eigen/Sparse>

#include <iostream>
#include <cstdio>

igl::bbw::BBWData::BBWData():
  partition_unity(false),
  W0(),
#ifndef IGL_NO_MOSEK
  mosek_data(),
#endif
  active_set_params(),
  qp_solver(QP_SOLVER_IGL_ACTIVE_SET),
  verbosity(0)
{
  // We know that the Bilaplacian is positive semi-definite
  active_set_params.Auu_pd = true;
}

void igl::bbw::BBWData::print()
{
  using namespace std;
  cout<<"partition_unity: "<<partition_unity<<endl;
  cout<<"W0=["<<endl<<W0<<endl<<"];"<<endl;
  cout<<"qp_solver: "<<QPSolverNames[qp_solver]<<endl;
}

template <
  typename DerivedV,
  typename DerivedEle,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::bbw::bbw(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedEle> & Ele,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  igl::bbw::BBWData & data,
  Eigen::PlainObjectBase<DerivedW> & W
  )
{
  using namespace std;
  using namespace Eigen;

  // number of domain vertices
  int n = V.rows();
  // number of handles
  int m = bc.cols();

  SparseMatrix<typename DerivedW::Scalar> L;
  cotmatrix(V,Ele,L);
  MassMatrixType mmtype = MASSMATRIX_TYPE_VORONOI;
  if(Ele.cols() == 4)
  {
    mmtype = MASSMATRIX_TYPE_BARYCENTRIC;
  }
  SparseMatrix<typename DerivedW::Scalar> M;
  SparseMatrix<typename DerivedW::Scalar> Mi;
  massmatrix(V,Ele,mmtype,M);

  invert_diag(M,Mi);

  // Biharmonic operator
  SparseMatrix<typename DerivedW::Scalar> Q = L.transpose() * Mi * L;

  W.derived().resize(n,m);
  if(data.partition_unity)
  {
    // Not yet implemented
    assert(false);
  }else
  {
    // No linear terms
    VectorXd c = VectorXd::Zero(n);
    // No linear constraints
    SparseMatrix<typename DerivedW::Scalar> A(0,n),Aeq(0,n),Aieq(0,n);
    VectorXd uc(0,1),Beq(0,1),Bieq(0,1),lc(0,1);
    // Upper and lower box constraints (Constant bounds)
    VectorXd ux = VectorXd::Ones(n);
    VectorXd lx = VectorXd::Zero(n);
    active_set_params eff_params = data.active_set_params;
    switch(data.qp_solver)
    {
      case QP_SOLVER_IGL_ACTIVE_SET:
      {
        if(data.verbosity >= 1)
        {
          cout<<"BBW: max_iter: "<<data.active_set_params.max_iter<<endl;
          cout<<"BBW: eff_max_iter: "<<eff_params.max_iter<<endl;
        }
        if(data.verbosity >= 1)
        {
          cout<<"BBW: Computing initial weights for "<<m<<" handle"<<
            (m!=1?"s":"")<<"."<<endl;
        }
        min_quad_with_fixed_data<typename DerivedW::Scalar > mqwf;
        min_quad_with_fixed_precompute(Q,b,Aeq,true,mqwf);
        min_quad_with_fixed_solve(mqwf,c,bc,Beq,W);
        // decrement
        eff_params.max_iter--;
        bool error = false;
        // Loop over handles
#pragma omp parallel for
        for(int i = 0;i<m;i++)
        {
          // Quicker exit for openmp
          if(error)
          {
            continue;
          }
          if(data.verbosity >= 1)
          {
#pragma omp critical
            cout<<"BBW: Computing weight for handle "<<i+1<<" out of "<<m<<
              "."<<endl;
          }
          VectorXd bci = bc.col(i);
          VectorXd Wi;
          // use initial guess
          Wi = W.col(i);
          SolverStatus ret = active_set(
              Q,c,b,bci,Aeq,Beq,Aieq,Bieq,lx,ux,eff_params,Wi);
          switch(ret)
          {
            case SOLVER_STATUS_CONVERGED:
              break;
            case SOLVER_STATUS_MAX_ITER:
              cerr<<"active_set: max iter without convergence."<<endl;
              break;
            case SOLVER_STATUS_ERROR:
            default:
              cerr<<"active_set error."<<endl;
              error = true;
          }
          W.col(i) = Wi;
        }
        if(error)
        {
          return false;
        }
        break;
      }
      case QP_SOLVER_MOSEK:
      {
#ifdef IGL_NO_MOSEK
        assert(false && "Use another QPSolver. Recompile without IGL_NO_MOSEK defined.");
        cerr<<"Use another QPSolver. Recompile without IGL_NO_MOSEK defined."<<endl;
        return false;
#else
        // Loop over handles
        for(int i = 0;i<m;i++)
        {
          if(data.verbosity >= 1)
          {
            cout<<"BBW: Computing weight for handle "<<i+1<<" out of "<<m<<
              "."<<endl;
          }
          VectorXd bci = bc.col(i);
          VectorXd Wi;
          // impose boundary conditions via bounds
          slice_into(bci,b,ux);
          slice_into(bci,b,lx);
          bool r = mosek_quadprog(Q,c,0,A,lc,uc,lx,ux,data.mosek_data,Wi);
          if(!r)
          {
            return false;
          }
          W.col(i) = Wi;
        }
#endif
        break;
      }
      default:
      {
        assert(false && "Unknown qp_solver");
        return false;
      }
    }
#ifndef NDEBUG
    const double min_rowsum = W.rowwise().sum().array().abs().minCoeff();
    if(min_rowsum < 0.1)
    {
      cerr<<"bbw.cpp: Warning, minimum row sum is very low. Consider more "
        "active set iterations or enforcing partition of unity."<<endl;
    }
#endif
  }

  return true;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template bool igl::bbw::bbw<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, igl::bbw::BBWData&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


/* ---------------------------------------------------------------------
**
** Copyright (C) 2017 Xiaoyu Wei
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is
** furnished to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in
** all copies or substantial portions of the Software.
**
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
** THE SOFTWARE.
**
** -------------------------------------------------------------------*/

#include <iostream>
#include <fstream>
#include <limits>
#include <string>
#include <array>
#include <cmath>

#include <boost/python/def.hpp>
#include <boost/python/docstring_options.hpp>
#include <boost/python/module.hpp>
#include <boost/python/numpy.hpp>

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/lac/vector.h>

namespace p  = boost::python;
namespace np = boost::python::numpy;
namespace d  = dealii;

constexpr int        dim = 2;
constexpr double     a   = -1;
constexpr double     b   = 1;
const d::UpdateFlags update_flags =
    d::update_quadrature_points | d::update_JxW_values;

void greet() { std::cout << "Yey!" << std::endl; }

double l_infty_distance(d::Point<dim> & p1, d::Point<dim> & p2){
  double dist = std::numeric_limits<double>::max();
  for (unsigned int d=0; d < dim; ++d) {
    double tmp = std::abs(p1[d] - p2[d]);
    dist = tmp > dist ? dist : tmp;
  }
  return dist;
}

double compute_q_point_radii(d::Point<dim> q_point, std::array<d::Point<dim>,
    d::GeometryInfo<dim>::vertices_per_cell> & vertices){
  double rad = std::numeric_limits<double>::max();
  for(auto && v : vertices){
    double tmp = l_infty_distance(q_point, v);
    rad = tmp > rad ? rad : tmp;
  }
  return rad;
}

template<int dimension, int n_vertices>
d::Point<dimension> compute_barycenter(
    std::array<d::Point<dimension>, n_vertices> & vertices){

  d::Point<dimension> bc;
  for (unsigned int d=0; d < dimension; ++d) {
    bc[d] = 0;
  }

  for(auto && v : vertices){
    bc += v;
  }
  bc /= n_vertices;
  return bc;
}

template <int dimension>
class MeshGenerator {
public:
  d::Triangulation<dimension> triangulation;
  d::FE_Q<dimension>          fe;
  d::DoFHandler<dimension>    dof_handler;
  d::QGauss<dimension>              quadrature_formula;

  double box_a, box_b;

  // q: quad order
  // level: initial (uniform) level
  MeshGenerator(int q, int level):
      triangulation(d::Triangulation<dimension>::maximum_smoothing),
      fe(q),
      dof_handler(triangulation),
      quadrature_formula(q),
      box_a(a),
      box_b(b)
  {
    d::GridGenerator::hyper_cube(triangulation, box_a, box_b);
    if (level > 1) {
      triangulation.refine_global(level - 1);
    }
    this->dof_handler.distribute_dofs(fe);
  }

  // customized bounding box
  MeshGenerator(int q, int level, double aa, double bb):
      triangulation(d::Triangulation<dimension>::maximum_smoothing),
      fe(q),
      dof_handler(triangulation),
      quadrature_formula(q),
      box_a(aa),
      box_b(bb)
  {
    d::GridGenerator::hyper_cube(triangulation, box_a, box_b);
    if (level > 1) {
      triangulation.refine_global(level - 1);
    }
    this->dof_handler.distribute_dofs(fe);
  }

  // NOTE: Copy constructor is not supported since dealii::DoFHandler is not
  // copy constructable, but a dummy copy constructor is required by
  // boost.python to build the code.
  MeshGenerator(const MeshGenerator<dimension>&):
    MeshGenerator(1, 1){};

  std::string greet() { return "Hello from MeshGen."; }

  void generate_gmsh(p::object &fn) {
    std::string filename = p::extract<std::string>(fn);
    std::ofstream output_file(filename);
    d::GridOut().write_msh(this->triangulation, output_file);
  }

  np::ndarray get_q_points() {
    np::dtype dtype = np::dtype::get_builtin<double>();

    const unsigned int n_q_points = this->quadrature_formula.size();
    const int total_n_q_points = triangulation.n_active_cells() * n_q_points;
    Py_intptr_t pt_shape[2] = {total_n_q_points, dimension};

    d::FEValues<dimension>   fe_values(this->fe, this->quadrature_formula,
        d::update_quadrature_points);
    np::ndarray points = np::zeros(2, pt_shape, dtype);
    auto ptp           = reinterpret_cast<double*>(points.get_data());

    auto cell = dof_handler.begin_active();
    auto endc = dof_handler.end();
    for (; cell != endc; ++cell) {
      fe_values.reinit(cell);
      std::vector<d::Point<dimension>> q_points = fe_values.get_quadrature_points();

      for (auto&& point : q_points) {
        for (unsigned int d = 0; d < dimension; ++d) {
          // For a preferred ordering of quad points
          // This does not change anything but the ordering due to the symmetry
          // in each direction (as long as the subdivided boxes are cubes), not
          // even weights are changed.
          *ptp = point[dim-1-d];
          ++ptp;
        }
      }
    }

    return points;
  }

  np::ndarray get_q_weights(){
    np::dtype dtype = np::dtype::get_builtin<double>();

    const unsigned int n_q_points = this->quadrature_formula.size();
    const int total_n_q_points = this->triangulation.n_active_cells() * n_q_points;
    Py_intptr_t w_shape[1]       = {total_n_q_points};

    d::FEValues<dimension>   fe_values(this->fe, this->quadrature_formula,
        d::update_JxW_values);
    np::ndarray weights = np::zeros(1, w_shape, dtype);
    auto ptw = reinterpret_cast<double*>(weights.get_data());

    auto cell = this->dof_handler.begin_active();
    auto endc = this->dof_handler.end();
    for (; cell != endc; ++cell) {
      fe_values.reinit(cell);
      std::vector<double> q_weights;

      for (unsigned int q_index = 0; q_index < n_q_points; ++q_index) {
        q_weights.push_back(fe_values.JxW(q_index));
      }

      for (auto&& weight : q_weights) {
          *ptw = weight;
          ++ptw;
      }
    }

    return weights;
  }

  np::ndarray get_cell_measures(){
    np::dtype dtype = np::dtype::get_builtin<double>();
    Py_intptr_t m_shape[1] = {this->triangulation.n_active_cells()};
    np::ndarray measures = np::zeros(1, m_shape, dtype);
    auto ptm = reinterpret_cast<double*>(measures.get_data());

    auto cell = this->dof_handler.begin_active();
    auto endc = this->dof_handler.end();
    for (; cell != endc; ++cell) {
      *ptm = cell->measure();
      ++ptm;
    }

    return measures;
  }

  np::ndarray get_cell_centers(){
    np::dtype dtype = np::dtype::get_builtin<double>();
    Py_intptr_t c_shape[2] = {this->triangulation.n_active_cells(), dimension};
    np::ndarray centers = np::zeros(2, c_shape, dtype);
    auto ptc = reinterpret_cast<double*>(centers.get_data());

    auto cell = this->dof_handler.begin_active();
    auto endc = this->dof_handler.end();
    for (; cell != endc; ++cell) {
      std::array<d::Point<dimension>,d::GeometryInfo<dimension>::vertices_per_cell> vertices;
      for (unsigned int v=0; v<d::GeometryInfo<dimension>::vertices_per_cell; ++v) {
        vertices[v] = cell->vertex(v);
      }
      auto barycenter = compute_barycenter<dimension,
                          d::GeometryInfo<dimension>::vertices_per_cell>(vertices);
      for (unsigned int d = 0; d < dimension; ++d) {
        *ptc = barycenter[dimension-1-d];
        ++ptc;
      }
    }

    return centers;
  }

  // Interface for dealii::GridRefinement::refine_and_coarsen_fixed_number
  void refine_and_coarsen_fixed_number(double* criteria,
                                       const double top_fraction_of_cells,
                                       const double bottom_fraction_of_cells) {
    d::Vector<double> ctr;
    ctr.reinit(this->triangulation.n_active_cells());
    double* ptc = criteria;
    for (unsigned int c = 0; c < this->triangulation.n_active_cells(); ++c){
      ctr[c] = *ptc;
      ++ptc;
    }

    d::GridRefinement::refine_and_coarsen_fixed_number (this->triangulation,
                                                        ctr,
                                                        top_fraction_of_cells,
                                                        bottom_fraction_of_cells);
  }

  int n_active_cells() {
    return this->triangulation.n_active_cells();
  }

  // Do both preparation for refinement and coarsening as well as mesh smoothing.
  // The function returns whether some cells' flagging has been changed in the process.
  bool prepare_coarsening_and_refinement() {
    return this->triangulation.prepare_coarsening_and_refinement();
  }

  void execute_coarsening_and_refinement() {
    this->triangulation.execute_coarsening_and_refinement();
    this->dof_handler.distribute_dofs(fe);
  }

  // Driver function for mesh adaptivity
  int update_mesh(np::ndarray const & criteria,
                   const double top_fraction_of_cells,
                   const double bottom_fraction_of_cells) {
    if (criteria.get_dtype() != np::dtype::get_builtin<double>()) {
        PyErr_SetString(PyExc_TypeError, "Incorrect array data type in the criteria");
        p::throw_error_already_set();
    }
    if (criteria.get_nd() != 1) {
        PyErr_SetString(PyExc_TypeError, "Incorrect number of dimensions of the criteria");
        p::throw_error_already_set();
    }

    // Refining top 1/3 in 2D doubles the number of cells
    double * iter = reinterpret_cast<double*>(criteria.get_data());
    this->refine_and_coarsen_fixed_number(iter, top_fraction_of_cells,
                                          bottom_fraction_of_cells);

    this->prepare_coarsening_and_refinement();
    this->execute_coarsening_and_refinement();

    return this->n_active_cells();
  }

  // Show some info about the mesh
  void print_info() {
    std::cout << "Number of active cells: " << this->triangulation.n_active_cells()
              << std::endl;

    d::FEValues<dimension>   fe_values(fe, quadrature_formula, d::update_quadrature_points);
    const unsigned int n_q_points = quadrature_formula.size();
    std::cout << "Number of quad points per cell: " << n_q_points << std::endl;
  }

};
typedef MeshGenerator<2> MeshGen2D;
typedef MeshGenerator<3> MeshGen3D;

// A legacy one-stop mesh generation function, returns quad points, weights
// and some spacing info within a tuple.
template <int dim>
p::tuple make_uniform_cubic_grid_details(int q, int level) {
  np::dtype dtype = np::dtype::get_builtin<double>();

  d::Triangulation<dim> triangulation;
  d::FE_Q<dim>          fe(q);
  d::DoFHandler<dim>    dof_handler(triangulation);
  d::QGauss<dim>        quadrature_formula(q);

  d::GridGenerator::hyper_cube(triangulation, a, b);
  if (level > 1) {
    triangulation.refine_global(level - 1);
  }
  dof_handler.distribute_dofs(fe);
  //std::cout << "Number of active cells: " << triangulation.n_active_cells()
            //<< std::endl;

  d::FEValues<dim>   fe_values(fe, quadrature_formula, update_flags);
  const unsigned int n_q_points = quadrature_formula.size();
  //std::cout << "Number of quad points per cell: " << n_q_points << std::endl;

  const int   total_n_q_points = triangulation.n_active_cells() * n_q_points;
  Py_intptr_t pt_shape[2]      = {total_n_q_points, dim};
  Py_intptr_t w_shape[1]       = {total_n_q_points};
  // Quad points
  np::ndarray points           = np::zeros(2, pt_shape, dtype);
  // Quad weights
  np::ndarray weights          = np::zeros(1, w_shape, dtype);
  // Distance (l_infty) to the closest cell vertex
  // (used for reconstructing the mesh in boxtree)
  np::ndarray radii            = np::zeros(1, w_shape, dtype);
  auto        ptp              = reinterpret_cast<double*>(points.get_data());
  auto        ptw              = reinterpret_cast<double*>(weights.get_data());
  auto        ptr              = reinterpret_cast<double*>(radii.get_data());

  // For margins
  std::array<double,dim*2> margins;
  std::array<double*, dim*2> mpts;
  margins.fill(std::numeric_limits<double>::max());

  auto cell = dof_handler.begin_active();
  auto endc = dof_handler.end();
  for (; cell != endc; ++cell) {
    fe_values.reinit(cell);
    std::vector<d::Point<dim>> q_points = fe_values.get_quadrature_points();
    std::vector<double>        q_weights;
    std::array<d::Point<dim>,d::GeometryInfo<dim>::vertices_per_cell> vertices;

    for (unsigned int q_index = 0; q_index < n_q_points; ++q_index) {
      q_weights.push_back(fe_values.JxW(q_index));
    }

    for (unsigned int v=0; v<d::GeometryInfo<dim>::vertices_per_cell; ++v) {
      vertices[v] = cell->vertex(v);
    }

    for (auto&& point : q_points) {
      for (unsigned int d = 0; d < dim; ++d) {
        // For a preferred ordering of quad points
        // This does not change anything but the ordering due to the symmetry
        // in each direction (as long as the subdivided boxes are cubes), not
        // even weights are changed.
        *ptp = point[dim-1-d];
        ++ptp;
      }
      if( std::abs(point[0] - a) < margins[0] ){
        margins[0] = std::abs(point[0] - a);
        mpts[0] = ptr;
      }
      if( std::abs(point[0] - b) < margins[1] ){
        margins[1] = std::abs(point[0] - b);
        mpts[1] = ptr;
      }
      if( std::abs(point[1] - a) < margins[2] ){
        margins[2] = std::abs(point[1] - a);
        mpts[2] = ptr;
      }
      if( std::abs(point[1] - b) < margins[3] ){
        margins[3] = std::abs(point[1] - b);
        mpts[3] = ptr;
      }
      ++ptr;
    }

    for (auto&& weight : q_weights) {
      *ptw = weight;
      ++ptw;
    }
  }

  for (unsigned int i=0; i < dim*2; ++i) {
    *(mpts[i]) = margins[i];
  }

  p::tuple result = p::make_tuple(points, weights, radii);
  return result;
}

p::tuple make_uniform_cubic_grid(int q, int level, int dim) {
  if (dim == 1) {
    return make_uniform_cubic_grid_details<1>(q, level);
  }
  else if (dim == 2) {
    return make_uniform_cubic_grid_details<2>(q, level);
  }
  else if (dim == 3) {
    return make_uniform_cubic_grid_details<3>(q, level);
  }
  else {
    std::cout << "Dimension must be 1,2 or 3." << std::endl;
    return make_uniform_cubic_grid_details<3>(q, level);
  }
}

BOOST_PYTHON_MODULE(meshgen_dealii) {
  using namespace boost::python;
  Py_Initialize();
  np::initialize();

  docstring_options doc_options(/*show_all=*/true);

  def("greet", greet,
      "Greentings! This module handles generation of quadrature points & "
      "weights.\n\nThe points and weights follow Legendre-Gauss quadrature "
      "rules.");

  def("make_uniform_cubic_grid", make_uniform_cubic_grid,
      (p::arg("degree") = 3, p::arg("level") = 2, p::arg("dim") = 2), "Make a simple cubic grid.");

  class_<MeshGen2D>("MeshGen2D", init<int, int>())
        .def(init<int, int, double, double>())
        .def("greet", &MeshGen2D::greet)
        .def("get_q_points", &MeshGen2D::get_q_points)
        .def("get_q_weights", &MeshGen2D::get_q_weights)
        .def("get_cell_centers", &MeshGen2D::get_cell_centers)
        .def("get_cell_measures", &MeshGen2D::get_cell_measures)
        .def("n_active_cells", &MeshGen2D::n_active_cells)
        .def("prepare_coarsening_and_refinement", &MeshGen2D::prepare_coarsening_and_refinement)
        .def("execute_coarsening_and_refinement", &MeshGen2D::execute_coarsening_and_refinement)
        .def("update_mesh", &MeshGen2D::update_mesh)
        .def("print_info", &MeshGen2D::print_info)
        .def("generate_gmsh", &MeshGen2D::generate_gmsh)
    ;

  class_<MeshGen3D>("MeshGen3D", init<int, int>())
        .def(init<int, int, double, double>())
        .def("greet", &MeshGen3D::greet)
        .def("get_q_points", &MeshGen3D::get_q_points)
        .def("get_q_weights", &MeshGen3D::get_q_weights)
        .def("get_cell_centers", &MeshGen3D::get_cell_centers)
        .def("get_cell_measures", &MeshGen3D::get_cell_measures)
        .def("n_active_cells", &MeshGen3D::n_active_cells)
        .def("prepare_coarsening_and_refinement", &MeshGen3D::prepare_coarsening_and_refinement)
        .def("execute_coarsening_and_refinement", &MeshGen3D::execute_coarsening_and_refinement)
        .def("update_mesh", &MeshGen3D::update_mesh)
        .def("print_info", &MeshGen3D::print_info)
        .def("generate_gmsh", &MeshGen3D::generate_gmsh)
    ;
}


/**
 * @file shapeMatching.cpp
 * @brief ShapeMatching plugin for Maya
 * @section LICENSE The MIT License
 * @section  requirements:  Eigen library, Maya
 * @section Limitation: the shapes must be connected
 * @version 0.10
 * @date  1/Nov/2013
 * @author Shizuo KAJI
 */

#pragma comment(linker, "/export:initializePlugin /export:uninitializePlugin")

#include "StdAfx.h"

#include <maya/MFnPlugin.h>

#include <numeric>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

class ShapeMatching : public MPxDeformerNode
{
public:
    ShapeMatching() {};
    virtual MStatus deform( MDataBlock& data, MItGeometry& itGeo, const MMatrix &localToWorldMatrix, unsigned int mIndex );
    static  void*   creator();
    static  MStatus initialize();
	static MTypeId id;
    static MString nodeName;
	static MObject aStartShape;
    static MObject aActive;
	static MObject aSlider;
	static MObject aDeltaTime;
	static MObject aStiffness;
	static MObject aAttenuation;
private:
    MatrixXd current, velocity;
    Matrix3d rotationPart(const Matrix3d m);
};

MTypeId ShapeMatching::id( 0x00000020 );
MString ShapeMatching::nodeName( "shapeMatching" );
MObject ShapeMatching::aStartShape;
MObject ShapeMatching::aSlider;
MObject ShapeMatching::aActive;
MObject ShapeMatching::aDeltaTime;
MObject ShapeMatching::aStiffness;
MObject ShapeMatching::aAttenuation;

void* ShapeMatching::creator() { return new ShapeMatching; }

// Compute
MStatus ShapeMatching::deform( MDataBlock& data, MItGeometry& itGeo, const MMatrix &localToWorldMatrix, unsigned int mIndex ){
    MStatus status;
//    MThreadUtils::syncNumOpenMPThreads();    // for OpenMP

    // read start shape
    MObject oStartShape = data.inputValue( aStartShape ).asMesh();
    if ( oStartShape.isNull() )    {
        return MS::kSuccess;
    }
    MFnMesh fnStartShape( oStartShape, &status );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    MPointArray endPoints, pts;
    itGeo.allPositions(pts);
    fnStartShape.getPoints( endPoints );
    // read attributes
    MDataHandle hSlider = data.inputValue( aSlider );
    bool active = data.inputValue( aActive ).asBool();
    float delta = data.inputValue( aDeltaTime ).asFloat();
    float stiffness = data.inputValue( aStiffness ).asFloat();
    float attenution = data.inputValue( aAttenuation ).asFloat();
    int num = pts.length();
    // first-time setup
    if (!active){
		current = MatrixXd(3,num);
		velocity = MatrixXd::Zero(3,num);
        for (int i = 0; i < num; i++) {
            current(0,i) = pts[i].x;
            current(1,i) = pts[i].y;
            current(2,i) = pts[i].z;
        }
        return MS::kSuccess;
    }
    // number of current and end points must be equal
    if (endPoints.length() != num){
        return MS::kSuccess;
    }
    // load end shape
    MatrixXd end(3,num);
    for (int i = 0; i < num; i++) {
        end(0,i) = endPoints[i].x;
        end(1,i) = endPoints[i].y;
        end(2,i) = endPoints[i].z;
    }
    // compute next step
    Vector3d current_center = current.rowwise().mean();
    Vector3d end_center = end.rowwise().mean();

    // prepare moment matrix
    current.colwise() -= current_center;
    end.colwise() -= end_center;
    Matrix3d moment = end * current.transpose();
	// Update current vertices and velocity of vertices
    velocity += delta * stiffness * (rotationPart(moment) * end - current);
    current += delta * velocity;
    velocity *= attenution;
    current.colwise() += current_center;

    // update points
    for (int i = 0; i < num; i++) {
        pts[i].x = current(0,i);
        pts[i].y = current(1,i);
        pts[i].z = current(2,i);
    }
    itGeo.setAllPositions(pts);

	return MS::kSuccess;
}

// Polar decomposition
Matrix3d ShapeMatching::rotationPart(const Matrix3d m){
    Matrix3d A= m*m.transpose();
	SelfAdjointEigenSolver<Matrix3d> eigensolver;
	eigensolver.computeDirect(A);
    Vector3d s = eigensolver.eigenvalues();
    Matrix3d U = Matrix3d(eigensolver.eigenvectors());
    s << sqrtf(s[0]), sqrtf(s[1]), sqrtf(s[2]);
    DiagonalMatrix<double,3> D(1.0f/s[0], 1.0f/s[1], 1.0f/s[2]);
    return m * U*D*U.transpose();
}

// setup attributes
MStatus ShapeMatching::initialize(){
    MFnTypedAttribute tAttr;
	MFnNumericAttribute nAttr;

	aStartShape = tAttr.create( "startShape", "ss", MFnData::kMesh );
    addAttribute( aStartShape );
    attributeAffects( aStartShape, outputGeom );
	aSlider = nAttr.create( "slider", "slider", MFnNumericData::kFloat, 0.0 );
    addAttribute( aSlider );
    attributeAffects( aSlider, outputGeom );
	aActive = nAttr.create( "active", "active", MFnNumericData::kBoolean, 0 );
    addAttribute( aActive );
    attributeAffects( aActive, outputGeom );
	aDeltaTime = nAttr.create( "delta", "delta", MFnNumericData::kFloat, 0.01 );
    addAttribute( aDeltaTime );
	aStiffness = nAttr.create( "stiffness", "stf", MFnNumericData::kFloat, 1.0 );
    addAttribute( aStiffness );
	aAttenuation = nAttr.create( "attenuation", "att", MFnNumericData::kFloat, 0.9 );
    addAttribute( aAttenuation );

	return MS::kSuccess;
}

// (un)init plugin
MStatus initializePlugin( MObject obj ){
    MStatus status;
    MFnPlugin plugin( obj, "CREST", "0.1", "Any");
    status = plugin.registerNode( ShapeMatching::nodeName, ShapeMatching::id, ShapeMatching::creator, ShapeMatching::initialize, MPxNode::kDeformerNode );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    return status;
}
MStatus uninitializePlugin( MObject obj ){
    MStatus   status;
    MFnPlugin plugin( obj );
    status = plugin.deregisterNode( ShapeMatching::id );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    return status;
}


#include <iostream>
#include <time.h>
#include <algorithm>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <LEDA/graph/graph.h>
#include <LEDA/graph/shortest_path.h>

using namespace std;
using namespace boost;
using namespace leda;

// Define the boost edge weight property
typedef property<edge_weight_t, double> EdgeWeightProperty;

// Define the boost directed graph: std::vector, std::vector, directed, no vertex property, double edge property, no graph property, std::list
typedef adjacency_list<vecS, vecS, directedS, no_property, EdgeWeightProperty, no_property, listS> DirectedGraph;

// Define the vertex class as vertex_desciptor
typedef graph_traits<DirectedGraph>::vertex_descriptor Vertex;

// Define the edge class as edge_desciptor
typedef graph_traits<DirectedGraph>::edge_descriptor Edge;

// Define the edge iterator as edge_iterator
typedef graph_traits<DirectedGraph>::edge_iterator EdgeIterator;

// Define the out edge iterator as edge_iterator
typedef graph_traits<DirectedGraph>::out_edge_iterator OutEdgeIterator;

// Define the edge iterator as edge_iterator
typedef graph_traits<DirectedGraph>::vertex_iterator VertexIterator;

// Define the edge weight map as a property map
typedef property_map<DirectedGraph, edge_weight_t>::type EdgeWeightMap;

/**
 * Calculates the heuristic value for the @param selectedVertex and @param targetVertex
 * @param Vertex The selected vertex
 * @param Vertex The target vertex
 * @return The heuristic value
 */
inline double HeuristicFunction(Vertex selectedVertex, Vertex targetVertex)
{
	// If the vertexes are the same...
	if(targetVertex == selectedVertex)
		// Return the heuristic
		return 0;

	// Calculate the heuristic
	double heuristicValue = selectedVertex - targetVertex;

	// Return the heuristic
	return abs(heuristicValue);
}

// The struct for the bool operation overloading
struct PairCompare
{
	/**
	 * Compares the pairs
	 * @param right The rightt operand
	 * @param left The left operand
	 * @return Comparison boolean value
	 */
	bool operator()(const pair<Vertex, double>& right, const pair<Vertex, double>& left)
	{
		return right.second < left.second;
	}
};

/**
 * Executes the ALT algorithm, namely a A* algorithm implementation, using landmarks and the triangle inequality
 * @param DirectedGraph The boost directed Graph
 * @param Vertex The starting vertex
 * @param Vertex The target vertex
 * @return True if the target vertex is found, or false if the target vertex is not found
 */
bool ALT(DirectedGraph& directedGraph, Vertex startingVertex, Vertex targetVertex)
{
	// Initialize the property map that contain the edges's weights
	EdgeWeightMap boostEdgeWeightMap = get(edge_weight, directedGraph);

	// Declare a map for the the vertexes and the cost to visit them
	std::map<Vertex, double> vertexVisitCostMap;

	// Declare a map for the the vertexes and the heuristic cost to visit them
	std::map<Vertex, double> vertexHeuristicCostMap;

	// Declare the boost vertex iterators
	VertexIterator vertexIteratorBegin, vertexIteratorEnd;

	// For every vertex in the boost directed graph...
	for(tie(vertexIteratorBegin, vertexIteratorEnd) = vertices(directedGraph); vertexIteratorBegin != vertexIteratorEnd; vertexIteratorBegin++)
	{
		// Set the vertex initial cost to DBL_MAX
		vertexVisitCostMap.insert(pair<Vertex, double>(*vertexIteratorBegin, DBL_MAX));

		// Set the vertex initial cost to DBL_MAX
		vertexHeuristicCostMap.insert(pair<Vertex, double>(*vertexIteratorBegin, DBL_MAX));
	}

	// Declare the boost out edge iterators
	OutEdgeIterator edgeIteratorBegin, edgeIteratorEnd;

	// Declare the vector for possible vertex successors
	std::vector<Vertex> possibleVertexSuccesorVector;

	// Insert the starting vertex
	possibleVertexSuccesorVector.push_back(startingVertex);

	// Set the starting vertex visit cost to 0
	vertexVisitCostMap[startingVertex] = 0;

	// Set the starting vertex heuristic cost
	vertexHeuristicCostMap[startingVertex] = HeuristicFunction(startingVertex, startingVertex);

	// While there is a vertex to search...
	while (!possibleVertexSuccesorVector.empty())
	{
		// Get an iterator that point to the vertex with the minimum heuristic value
		auto it = *min_element(vertexHeuristicCostMap.begin(), vertexHeuristicCostMap.end(), PairCompare());

		// Get the vertex from the iterator
		Vertex currentVertex = it.first;

		// If the target node was reached...
		if (currentVertex == targetVertex)
		{
			// Return true
			return true;
		}

		// Search the target vertex
		auto iterator = find(possibleVertexSuccesorVector.begin(), possibleVertexSuccesorVector.end(), currentVertex);

		// If the element doesn't exist...
		if(iterator == possibleVertexSuccesorVector.end())
			// Clear the vector
			possibleVertexSuccesorVector.clear();
		else
			// Remove the current vertex
			possibleVertexSuccesorVector.erase(iterator, possibleVertexSuccesorVector.end());

		// For every out edge of the current vertex...
		for(tie(edgeIteratorBegin, edgeIteratorEnd) = out_edges(currentVertex, directedGraph); edgeIteratorBegin != edgeIteratorEnd; edgeIteratorBegin++)
		{
			// Get the current edge's target node
			Vertex currentEdgeTargetVertex = target(*edgeIteratorBegin, directedGraph);

			// Get the current edge's weight
			double currentEdgeWeight = boostEdgeWeightMap[*edgeIteratorBegin];

			// Calculate the target vertex visit cost
			double targetVertexVisitCost = vertexVisitCostMap[currentVertex] + currentEdgeWeight;

			// If the the path to this node is better than the previous one...
			if(targetVertexVisitCost < vertexVisitCostMap[currentEdgeTargetVertex])
			{
				// Set the visit cost to the edge's target
				vertexVisitCostMap[currentEdgeTargetVertex] = targetVertexVisitCost;

				// Set the heuristic cost to the edge's target
				vertexHeuristicCostMap[currentEdgeTargetVertex] = targetVertexVisitCost + HeuristicFunction(startingVertex, currentEdgeTargetVertex);

				// Search the target vertex
				auto iterator = find(possibleVertexSuccesorVector.begin(), possibleVertexSuccesorVector.end(), currentEdgeTargetVertex);

				// If the edge's target doesn't exist in the vector...
				if(iterator == possibleVertexSuccesorVector.end())
				{
					// Add it
					possibleVertexSuccesorVector.push_back(currentEdgeTargetVertex);
				}
			}
		}
	}

	// Return false
	return false;
}

/**
 * Copies @param LedaGraph to @param BoostDirectedGraph, using the @param LedaEdgeWeightMap
 * @param BoostDirectedGraph The boost directed graph
 * @param LedaGraph The leda directed graph
 * @param LedaEdgeWeightMap The edge array that contain the leda directed graph edges weights
*/
void CopyLedaGraphToBoostGraph(DirectedGraph& BoostDirectedGraph, leda::graph& LedaGraph, edge_array<double>& LedaEdgeWeightMap)
{
	// Create a new boost directed graph containing the smae number of nodes as the leda directed graph
	DirectedGraph boostGraph(LedaGraph.number_of_nodes());

	// Leda edge that will be used for iteration
	leda::edge iterationEdge;

	// For all edges in the leda directed graph
	forall_edges(iterationEdge, LedaGraph)
	{
		// Get the source node of the edge
		node source = LedaGraph.source(iterationEdge);

		// Get the target node of the edge
		node target = LedaGraph.target(iterationEdge);

		// Get the weight of the edge
		double currentEdgeWeight = LedaEdgeWeightMap[iterationEdge];

		// Add the edge in the boost directed graph
		add_edge(LedaGraph.index(source), LedaGraph.index(target), currentEdgeWeight, boostGraph);
	}

	// Update the boost directed graph
	BoostDirectedGraph = boostGraph;
}

// Main function
int main()
{
	#pragma region Initialization

	// Create an empty boost directed graph
	DirectedGraph boostDirectedGraph;

	// Create an empty leda directed graph
	leda::graph ledaDirectedGraph;

	//Create an empty edge array
	edge_array<double> ledaEdgeWeightArray;

	// User graph option
	std::string graphOption;

	// Number of nodes
	int numberOfNodes;

	cout << "Choose the testing graph between grid, complete or random." << endl;

	// Read the graph type
	cin >> graphOption;

	cout << "Enter the number of nodes." << endl;

	// Read the number of nodes
	cin >> numberOfNodes;

	// If the grid graph is selected...
	if(graphOption == "grid")
	{
		// Create a grid graph
		grid_graph(ledaDirectedGraph, numberOfNodes);
	}
	else
	{
		// If the random graph is selected...
		if(graphOption == "random")
		{
			// Calculate the number of edges
			int numberOfEdges = ceil(numberOfNodes * (numberOfNodes - 1));

			// Generate a random directed graph
			random_graph(ledaDirectedGraph, numberOfNodes, numberOfEdges, false, true, true);

			// Make the graph cohesive
			Make_Connected(ledaDirectedGraph);
		}
		else
		{
			// If the complete graph is selected...
			if(graphOption == "complete")
			{
				// Create a complete graph
				complete_graph(ledaDirectedGraph, numberOfNodes);
			}
			else
			{
				cout << "Choose between grid, complete or random." << endl;

				// Exit if another option is selected
				exit(0);
			}
		}
	}

	// Intialise an edge array that will contain the leda graph edges weights
	edge_array<double> edgeWeightArray(ledaDirectedGraph);

	// Copy the edge array
	ledaEdgeWeightArray = edgeWeightArray;

	// Initialise a random seed
	srand(time(NULL));

	// Edge that will be used for the iteration
	leda::edge iterationEdge;

	// For every edge in the undirected graph...
	forall_edges(iterationEdge, ledaDirectedGraph)
	{
		// Assign random integer values as costs between 10 and 10000
		ledaEdgeWeightArray[iterationEdge] = (rand() % 100) + 1;
	}

	// Get the grid graph number of nodes
	numberOfNodes = ledaDirectedGraph.number_of_nodes();

	// Copy the leda directed graph to the boost directed graph
	CopyLedaGraphToBoostGraph(boostDirectedGraph, ledaDirectedGraph, ledaEdgeWeightArray);

	// Initialise a property map that contain the boost graph edges weights
	EdgeWeightMap boostEdgeWeightMap = get(edge_weight, boostDirectedGraph);

	// Choose a random node from the Leda directed graph
	node startingRandomLedaNode = ledaDirectedGraph.choose_node();

	// Choose a random node from the Leda directed graph
	node targetRandomLedaNode = ledaDirectedGraph.choose_node();

	// Get the index of the Boost directed graph vertex that correspond to the appropriate Leda random chosen node
	Vertex startingRandomBoostVertex = vertex(ledaDirectedGraph.index(startingRandomLedaNode), boostDirectedGraph);

	// Get the index of the Boost directed graph vertex that correspond to the appropriate Leda random chosen node
	Vertex targetRandomBoostVertex = vertex(ledaDirectedGraph.index(targetRandomLedaNode), boostDirectedGraph);

	// Initialise a node array the will contain the last edge on a shortest path from the starting node to a node
	node_array<leda::edge> ledaPredecessorNodeArray(ledaDirectedGraph);

	// Initialise a node array the will contain the shortest path langth from the starting node to a node
	node_array<double> ledaDistanceNodeArray(ledaDirectedGraph);

	#pragma endregion Initialization

	#pragma region Simulation

	// Initialise the starting CPU time
	float CPUTime = used_time();

	// Execute user defined ALT function
	ALT(boostDirectedGraph, startingRandomBoostVertex, targetRandomBoostVertex);

	// Print the user defined ALT function execution time
	cout << "User defined ALT function execution time: " << used_time(CPUTime) << " seconds."<< endl;

	// Execute leda defined shortest path function
	SHORTEST_PATH_T(ledaDirectedGraph, startingRandomLedaNode, ledaEdgeWeightArray, ledaDistanceNodeArray, ledaPredecessorNodeArray);

	// Execute leda defined compute shortest path function
	COMPUTE_SHORTEST_PATH(ledaDirectedGraph, startingRandomLedaNode, targetRandomLedaNode, ledaPredecessorNodeArray);

	// Print the leda defined compute shortest path function execution time
	cout << "Leda defined compute shortest path function execution time: " << used_time(CPUTime) << " seconds."<< endl;

	#pragma endregion Simulation

	// Return 0
	return 0;
}

Project Τεχνολογίες Υλοποίησης Αλγορίθμων/Τελική Άσκηση/TelikiErgasia.cpp


#include "SSDR.h"
#include <limits>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include "QuadProg.h"
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>

using namespace Eigen;

namespace SSDR {

    double ComputeApproximationErrorSq(const Output& output, const Input& input, const Parameter& param)
    {
        std::vector<double> errsq(input.numExamples);
        double rsqsum = 0;
        for (int s = 0; s < input.numExamples; ++s)
        {
            const int numVertices = input.numVertices;
            const int numIndices = param.numMaxInfluences;
            const int numBones = output.numBones;
            const int numExamples = input.numExamples;

            for (int s = 0; s < numExamples; ++s)
            {
                for (int v = 0; v < numVertices; ++v)
                {
                    MVector residual = input.sample[s * numVertices + v];
                    const MPoint& p = input.bindModel[v];
                    for (int i = 0; i < numIndices; ++i)
                    {
                        const int b = output.index[v * numIndices + i];
                        const double w = output.weight[v * numIndices + i];
                        const MTransformationMatrix& rt = output.boneTrans[s * numBones + b];
                        residual -= w * MVector(p * rt.asMatrix());
                    }
                    rsqsum += residual * residual;
                }
            }
        }
        return rsqsum;
    }

    class WeightMapUpdator
    {
    private:
        Output* output;
        const Input* input;
        const Parameter* param;
        const MatrixXd* cem;
        const MatrixXd* cim;
        const VectorXd* cev;
        const VectorXd* civ;
        const MatrixXd* scem;
        const MatrixXd* scim;
        const VectorXd* sciv;
    public:
        WeightMapUpdator(Output* output_, const Input* input_, const Parameter* param_,
            const MatrixXd* cem_, const MatrixXd* cim_, const VectorXd* cev_, const VectorXd* civ_,
            const MatrixXd* scem_, const MatrixXd* scim_, const VectorXd* sciv_)
            : output(output_), input(input_), param(param_),
            cem(cem_), cim(cim_), cev(cev_), civ(civ_),
            scem(scem_), scim(scim_), sciv(sciv_)
        {
        }
        void operator ()(const tbb::blocked_range<int>& range) const
        {
            const int numVertices = input->numVertices;
            const int numExamples = input->numExamples;
            const int numIndices = param->numMaxInfluences;
            const int numBones = output->numBones;

            MatrixXd gm = MatrixXd::Zero(numBones, numBones), sgm = MatrixXd::Zero(numIndices, numIndices);
            VectorXd gv = VectorXd::Zero(numBones), sgv = VectorXd::Zero(numIndices);

            VectorXd weight = VectorXd::Zero(numBones), w0, sweight = VectorXd::Zero(numIndices);
            MatrixXd basis = MatrixXd::Zero(numBones, numExamples * 3), sbasis = MatrixXd::Zero(numIndices, numExamples * 3);
            VectorXd targetVertex = VectorXd::Zero(numExamples * 3);

            for (int v = range.begin(); v != range.end(); ++v)
            {
                const MPoint& restVertex = input->bindModel[v];
                for (int s = 0; s < numExamples; ++s)
                {
                    for (int b = 0; b < numBones; ++b)
                    {
                        const MTransformationMatrix& rt = output->boneTrans[s * numBones + b];
                        MPoint tv = restVertex * rt.asMatrix();
                        basis(b, s * 3 + 0) = tv.x;
                        basis(b, s * 3 + 1) = tv.y;
                        basis(b, s * 3 + 2) = tv.z;
                    }
                }
                for (int s = 0; s < numExamples; ++s)
                {
                    targetVertex[s * 3 + 0] = input->sample[s * numVertices + v].x;
                    targetVertex[s * 3 + 1] = input->sample[s * numVertices + v].y;
                    targetVertex[s * 3 + 2] = input->sample[s * numVertices + v].z;
                }
                gm = basis * basis.transpose();
                gv = -basis * targetVertex;

                double qperr = SolveQP(gm, gv, *cem, *cev, *cim, *civ, weight);
                assert(qperr != std::numeric_limits<double>::infinity());

                double weightSum = 0;
                for (int i = 0; i < numIndices; ++i)
                {
                    double maxw = -std::numeric_limits<double>::max();
                    int bestbone = -1;
                    for (int b = 0; b < numBones; ++b)
                    {
                        if (weight[b] > maxw)
                        {
                            maxw = weight[b];
                            bestbone = b;
                        }
                    }
                    if (maxw <= 0)
                    {
                        break;
                    }

                    output->index[v * numIndices + i] = bestbone;
                    output->weight[v * numIndices + i] = maxw;
                    weightSum += maxw;
                    weight[bestbone] = 0;
                }
                if (weightSum < 1.0f)
                {
                    for (int j = 0; j < numExamples * 3; ++j)
                    {
                        for (int i = 0; i < numIndices; ++i)
                        {
                            sbasis(i, j) = basis(output->index[v * numIndices + i], j);
                        }
                    }
                    sgm = sbasis * sbasis.transpose();
                    sgv = -sbasis * targetVertex;
                    qperr = SolveQP(sgm, sgv, *scem, *cev, *scim, *sciv, sweight);
                    if (qperr != std::numeric_limits<double>::infinity())
                    {
                        weightSum = 0;
                        for (int i = 0; i < numIndices; ++i)
                        {
                            weightSum += sweight[i];
                            output->weight[v * numIndices + i] = sweight[i];
                        }
                    }
                }
                for (int i = 0; i < numIndices; ++i)
                {
                    output->weight[v * numIndices + i] /= weightSum;
                }
            }
        }
    };
    void UpdateWeightMap(Output& output, const Input& input, const Parameter& param)
    {
        const int numBones = output.numBones;
        const int numIndices = param.numMaxInfluences;

        MatrixXd cem = MatrixXd::Zero(1, numBones);
        MatrixXd scem = MatrixXd::Zero(1, numIndices);
        VectorXd cev = VectorXd::Zero(1);
        MatrixXd cim = MatrixXd::Zero(numBones, numBones);
        MatrixXd scim = MatrixXd::Zero(numIndices, numIndices);
        VectorXd civ = VectorXd::Zero(numBones);
        VectorXd sciv = VectorXd::Zero(numIndices);
        for (int b = 0; b < numBones; ++b)
        {
            cem(0, b) = -1.0;
            cim(b, b) = 1.0;
            civ(b) = 0;
        }
        for (int i = 0; i < numIndices; ++i)
        {
            scem(0, i) = -1.0;
            scim(i, i) = 1.0;
            sciv(i) = 0;
        }
        cev(0) = 1.0;

        tbb::parallel_for(tbb::blocked_range<int>(0, input.numVertices),
            WeightMapUpdator(&output, &input, &param,
            &cem, &cim, &cev, &civ, &scem,
            &scim, &sciv));
    }

    MTransformationMatrix CalcPointsAlignment(size_t numPoints, std::vector<MPoint>::const_iterator ps, std::vector<MPoint>::const_iterator pd)
    {
        MTransformationMatrix transform;

        MPoint cs = MVector::zero, cd = MVector::zero;
        std::vector<MPoint>::const_iterator sit = ps;
        std::vector<MPoint>::const_iterator dit = pd;
        for (size_t i = 0; i < numPoints; ++i, ++sit, ++dit)
        {
            cs += *sit;
            cd += *dit;
        }
        cs = cs / numPoints;
        cd = cd / numPoints;

        if (numPoints < 3)
        {
            transform.setTranslation(cd - cs, MSpace::kTransform);
            return transform;
        }

        Matrix<double, 4, 4> moment;
        double sxx = 0, sxy = 0, sxz = 0, syx = 0, syy = 0, syz = 0, szx = 0, szy = 0, szz = 0;
        sit = ps;
        dit = pd;
        for (size_t i = 0; i < numPoints; ++i, ++sit, ++dit)
        {
            sxx += (sit->x - cs.x) * (dit->x - cd.x);
            sxy += (sit->x - cs.x) * (dit->y - cd.y);
            sxz += (sit->x - cs.x) * (dit->z - cd.z);
            syx += (sit->y - cs.y) * (dit->x - cd.x);
            syy += (sit->y - cs.y) * (dit->y - cd.y);
            syz += (sit->y - cs.y) * (dit->z - cd.z);
            szx += (sit->z - cs.z) * (dit->x - cd.x);
            szy += (sit->z - cs.z) * (dit->y - cd.y);
            szz += (sit->z - cs.z) * (dit->z - cd.z);
        }
        moment(0, 0) = sxx + syy + szz;
        moment(0, 1) = syz - szy;        moment(1, 0) = moment(0, 1);
        moment(0, 2) = szx - sxz;        moment(2, 0) = moment(0, 2);
        moment(0, 3) = sxy - syx;        moment(3, 0) = moment(0, 3);
        moment(1, 1) = sxx - syy - szz;
        moment(1, 2) = sxy + syx;        moment(2, 1) = moment(1, 2);
        moment(1, 3) = szx + sxz;        moment(3, 1) = moment(1, 3);
        moment(2, 2) = -sxx + syy - szz;
        moment(2, 3) = syz + szy;        moment(3, 2) = moment(2, 3);
        moment(3, 3) = -sxx - syy + szz;

        if (moment.norm() > 0)
        {
            EigenSolver<Matrix<double, 4, 4>> es(moment);
            int maxi = 0;
            for (int i = 1; i < 4; ++i)
            {
                if (es.eigenvalues()(maxi).real() < es.eigenvalues()(i).real())
                {
                    maxi = i;
                }
            }
            transform.setRotationQuaternion(
                es.eigenvectors()(1, maxi).real(),
                es.eigenvectors()(2, maxi).real(),
                es.eigenvectors()(3, maxi).real(),
                es.eigenvectors()(0, maxi).real());
        }

        MPoint cs0 = cs * transform.asMatrix();
        transform.setTranslation(cd - cs0, MSpace::kTransform);
        return transform;
    }

    void ComputeExamplePoints(std::vector<MPoint>& example, int sid, int bone, const Output& output, const Input& input, const Parameter& param)
    {
        const int numVertices = input.numVertices;
        const int numIndices = param.numMaxInfluences;
        const int numBones = output.numBones;
        for (int v = 0; v < numVertices; ++v)
        {
            example[v] = input.sample[sid * numVertices + v];
            const MPoint& s = input.bindModel[v];
            for (int i = 0; i < numIndices; ++i)
            {
                const int b = output.index[v * numIndices + i];
                if (b != bone)
                {
                    const double w = output.weight[v * numIndices + i];
                    const MTransformationMatrix& at = output.boneTrans[sid * numBones + b];
                    example[v] -= w * (s * at.asMatrix());
                }
            }
        }
    }

    void SubtractCentroid(std::vector<MPoint>& model, std::vector<MPoint>& example, MPoint& corModel, MPoint& corExample, const VectorXd& weight, const Output& output, const Input& input)
    {
        const int numVertices = input.numVertices;

        double wsqsum = 0;
        corModel = MVector::zero;
        corExample = MVector::zero;
        for (int v = 0; v < numVertices; ++v)
        {
            const double w = weight[v];
            corModel += w * w * input.bindModel[v];
            corExample += w * example[v];
            wsqsum += w * w;
        }
        corModel = corModel / wsqsum;
        corExample = corExample / wsqsum;
        for (int v = 0; v < numVertices; ++v)
        {
            model[v] = weight[v] * (input.bindModel[v] - corModel);
            example[v] -= weight[v] * corExample;
        }
    }

    class BoneTransformUpdator
    {
    private:
        Output* output;
        const Input* input;
        const Parameter* param;
        const VectorXd* weight;
        int bone;
    public:
        BoneTransformUpdator(Output* output_, const Input* input_, const Parameter* param_, const VectorXd* weight_)
            : bone(0), output(output_), input(input_), param(param_), weight(weight_)
        {
        }
        void ChangeBone(int b)
        {
            bone = b;
        }
        void operator ()(const tbb::blocked_range<int>& range) const
        {
            std::vector<MPoint> model(input->numVertices), example(input->numVertices);
            for (int s = range.begin(); s != range.end(); ++s)
            {
                ComputeExamplePoints(example, s, bone, *output, *input, *param);

                MPoint corModel(0, 0, 0), corExample(0, 0, 0);
                SubtractCentroid(model, example, corModel, corExample, *weight, *output, *input);

                MTransformationMatrix transform = CalcPointsAlignment(model.size(), model.begin(), example.begin());
                MVector d = corExample - corModel * transform.asMatrix();
                transform.setTranslation(d + transform.getTranslation(MSpace::kTransform), MSpace::kTransform);
                output->boneTrans[s * output->numBones + bone] = transform;
            }
        }
    };
    void UpdateBoneTransform(Output& output, const Input& input, const Parameter& param)
    {
        const int numVertices = input.numVertices;
        const int numExamples = input.numExamples;
        const int numIndices = param.numMaxInfluences;
        const int numBones = output.numBones;

        VectorXd boneWeight = VectorXd::Zero(numVertices);
        BoneTransformUpdator transformUpdator(&output, &input, &param, &boneWeight);
        tbb::blocked_range<int> blockedRange(0, numExamples);
        for (int bone = 0; bone < numBones; ++bone)
        {
            for (int v = 0; v < numVertices; ++v)
            {
                boneWeight[v] = 0;
                for (int i = 0; i < numIndices; ++i)
                {
                    if (output.index[v * numIndices + i] == bone)
                    {
                        boneWeight[v] = output.weight[v * numIndices + i];
                        break;
                    }
                }
            }
            transformUpdator.ChangeBone(bone);
            tbb::parallel_for(blockedRange, transformUpdator);
        }
    }

    void UpdateBoneTransform(std::vector<MTransformationMatrix>& boneTrans, int numBones, const Output& output, const Input& input, const Parameter& param)
    {
        const int numVertices = input.numVertices;
        const int numExamples = input.numExamples;
        const int numIndices = param.numMaxInfluences;

        std::vector<int> numBoneVertices(numBones, 0);
        for (int v = 0; v < numVertices; ++v)
        {
            ++numBoneVertices[output.index[v * numIndices + 0]];
        }
        std::vector<int> boneVertexId(numBones, 0);
        for (int i = 1; i < numBones; ++i)
        {
            boneVertexId[i] = boneVertexId[i - 1] + numBoneVertices[i - 1];
        }
        std::vector<MPoint> skin(numVertices, MPoint(0, 0, 0));
        std::vector<MPoint> anim(numVertices * numExamples, MPoint(0, 0, 0));
        for (int v = 0; v < numVertices; ++v)
        {
            const int bs = output.index[v * numIndices + 0];
            const int bd = boneVertexId[bs];
            skin[bd] = input.bindModel[v];
            for (int s = 0; s < numExamples; ++s)
            {
                anim[s * numVertices + bd] = input.sample[s * numVertices + v];
            }
            ++boneVertexId[bs];
        }
        boneVertexId[0] = 0;
        for (int i = 1; i < numBones; ++i)
        {
            boneVertexId[i] = boneVertexId[i - 1] + numBoneVertices[i - 1];
        }
        for (int b = 0; b < numBones; ++b)
        {
            for (int s = 0; s < numExamples; ++s)
            {
                boneTrans[s * numBones + b] = CalcPointsAlignment(numBoneVertices[b], skin.begin() + boneVertexId[b], anim.begin() + s * numVertices + boneVertexId[b]);
            }
        }
    }

    int BindVertexToBone(Output& output, std::vector<MTransformationMatrix>& boneTrans, const Input& input, const Parameter& param)
    {
        const int numVertices = input.numVertices;
        const int numExamples = input.numExamples;
        const int numIndices = param.numMaxInfluences;
        int numBones = static_cast<int>(boneTrans.size() / numExamples);

        std::vector<int> numBoneVertices(numBones, 0);
        std::vector<double> vertexError(numVertices, 0);

        for (int v = 0; v < numVertices; ++v)
        {
            int bestBone = 0;
            double minErr = std::numeric_limits<double>::max();
            const MPoint& bindModelPos = input.bindModel[v];
            for (int b = 0; b < numBones; ++b)
            {
                double errsq = 0;
                for (int s = 0; s < numExamples; ++s)
                {
                    MMatrix am = boneTrans[s * numBones + b].asMatrix();
                    MVector diff = input.sample[s * numVertices + v] - bindModelPos * am;
                    errsq += diff * diff;
                }
                if (errsq < minErr)
                {
                    bestBone = b;
                    minErr = errsq;
                }
            }
            ++numBoneVertices[bestBone];
            output.index[v * numIndices + 0] = bestBone;
            vertexError[v] = minErr;
        }

        std::vector<int>::iterator smallestBoneSize = std::min_element(numBoneVertices.begin(), numBoneVertices.end());
        while (*smallestBoneSize <= 0)
        {
            const int smallestBone = static_cast<int>(smallestBoneSize - numBoneVertices.begin());
            numBoneVertices.erase(numBoneVertices.begin() + smallestBone);
            for (int s = input.numExamples - 1; s >= 0; --s)
            {
                boneTrans.erase(boneTrans.begin() + s * numBones + smallestBone);
            }
            for (int v = 0; v < numVertices; ++v)
            {
                int b = output.index[v * numIndices + 0];
                if (b >= smallestBone)
                {
                    --output.index[v * numIndices + 0];
                }
            }
            --numBones;
            smallestBoneSize = std::min_element(numBoneVertices.begin(), numBoneVertices.end());
        }
        return static_cast<int>(numBoneVertices.size());
    }

    int ClusterInitialBones(Output& output, const Input& input, const Parameter& param)
    {
        const int numVertices = input.numVertices;
        const int numExamples = input.numExamples;
        const int numIndices = param.numMaxInfluences;

        std::fill(output.index.begin(), output.index.end(), 0);
        std::fill(output.weight.begin(), output.weight.end(), 0.0);
        for (int v = 0; v < numVertices; ++v)
        {
            output.weight[v * numIndices + 0] = 1.0;
        }

        int numClusters = 1;
        std::vector<MTransformationMatrix> boneTrans(numExamples);
        UpdateBoneTransform(boneTrans, numClusters, output, input, param);

        while (numClusters < param.numMinBones)
        {
            std::vector<MPoint> clusterCenter(numClusters, MPoint(0, 0, 0));
            std::vector<int> numBoneVertices(numClusters, 0);
            for (int v = 0; v < numVertices; ++v)
            {
                const int c = output.index[v * numIndices + 0];
                clusterCenter[c] += input.bindModel[v];
                ++numBoneVertices[c];
            }
            for (int c = 0; c < numClusters; ++c)
            {
                clusterCenter[c] = clusterCenter[c] / numBoneVertices[c];
            }

            std::vector<double> maxClusterError(numClusters, -std::numeric_limits<double>::max());
            std::vector<int> mostDistantVertex(numClusters, -1);
            for (int v = 0; v < numVertices; ++v)
            {
                const int c = output.index[v * numIndices + 0];
                double sumApproxErrorSq = 0;
                for (int s = 0; s < numExamples; ++s)
                {
                    MMatrix bm = boneTrans[s * numClusters + c].asMatrix();
                    MVector diff = input.sample[s * numVertices + v] - input.bindModel[v] * bm;
                    sumApproxErrorSq += diff * diff;
                }
                MVector d = input.bindModel[v] - clusterCenter[c];
                double errSq = sumApproxErrorSq * (d * d);
                if (errSq > maxClusterError[c])
                {
                    maxClusterError[c] = errSq;
                    mostDistantVertex[c] = v;
                }
            }
            int numPrevClusters = numClusters;
            for (int c = 0; c < numPrevClusters; ++c)
            {
                output.index[mostDistantVertex[c] * numIndices + 0] = numClusters++;
                --numBoneVertices[c];
                numBoneVertices.push_back(1);
            }
            boneTrans.resize(numExamples * numClusters);

            UpdateBoneTransform(boneTrans, numClusters, output, input, param);
            numClusters = BindVertexToBone(output, boneTrans, input, param);
        }
        return numClusters;
    }

#pragma region Decompose
    double Decompose(Output& output, const Input& input, const Parameter& param)
    {
        const int numVertices = input.numVertices;
        const int numExamples = input.numExamples;
        const int numIndices = param.numMaxInfluences;

        output.index.assign(numVertices * numIndices, 0);
        output.weight.assign(numVertices * numIndices, 0.0f);

        output.numBones = ClusterInitialBones(output, input, param);
        output.boneTrans.assign(numExamples * output.numBones, MTransformationMatrix::identity);
        UpdateBoneTransform(output.boneTrans, output.numBones, output, input, param);

        for (int loop = 0; loop < param.numMaxIterations; ++loop)
        {
            UpdateWeightMap(output, input, param);
            UpdateBoneTransform(output, input, param);
        }
        return ComputeApproximationErrorSq(output, input, param);
    }
#pragma endregion

} //namespace SSDR