/* -*- c++ -*- */
/*
 * Copyright 2009-2011,2013 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "histo_sink_f_impl.h"
#include <gnuradio/io_signature.h>
#include <boost/math/special_functions/round.hpp>

namespace gr {
  namespace wxgui {

    static float get_clean_num(float num)
    {
      if(num == 0)
        return 0;
      /* extract sign and exponent from num */
      int sign = (num < 0) ? -1 : 1; num = fabs(num);
      float exponent = floor(log10(num));
      /* search for closest number with base 1, 2, 5, 10 */
      float closest_num = 10*pow(10, exponent);
      if(fabs(num - 1*pow(10, exponent)) < fabs(num - closest_num))
        closest_num = 1*pow(10, exponent);
      if(fabs(num - 2*pow(10, exponent)) < fabs(num - closest_num))
        closest_num = 2*pow(10, exponent);
      if(fabs(num - 5*pow(10, exponent)) < fabs(num - closest_num))
        closest_num = 5*pow(10, exponent);
      return sign*closest_num;
    }

    histo_sink_f::sptr
    histo_sink_f::make(msg_queue::sptr msgq)
    {
      return gnuradio::get_initial_sptr
        (new histo_sink_f_impl(msgq));
    }

    histo_sink_f_impl::histo_sink_f_impl(msg_queue::sptr msgq)
      : sync_block("histo_sink_f",
                      io_signature::make(1, 1, sizeof(float)),
                      io_signature::make(0, 0, 0)),
        d_msgq(msgq), d_num_bins(11), d_frame_size(1000),
        d_sample_count(0), d_bins(NULL), d_samps(NULL)
    {
      //allocate arrays and clear
      set_num_bins(d_num_bins);
      set_frame_size(d_frame_size);
    }

    histo_sink_f_impl::~histo_sink_f_impl(void)
    {
      delete [] d_samps;
      delete [] d_bins;
    }

    int
    histo_sink_f_impl::work(int noutput_items,
                            gr_vector_const_void_star &input_items,
                            gr_vector_void_star &output_items)
    {
      const float *in = (const float*)input_items[0];
      gr::thread::scoped_lock guard(d_mutex);  // hold mutex for duration of this function
      for(unsigned int i = 0; i < (unsigned int)noutput_items; i++) {
        d_samps[d_sample_count] = in[i];
        d_sample_count++;
        /* processed a frame? */
        if(d_sample_count == d_frame_size) {
          send_frame();
          clear();
        }
      }
      return noutput_items;
    }

    void
    histo_sink_f_impl::send_frame(void)
    {
      /* output queue full, drop the data */
      if(d_msgq->full_p())
        return;
      /* find the minimum and maximum */
      float minimum = d_samps[0];
      float maximum = d_samps[0];
      for(unsigned int i = 0; i < d_frame_size; i++) {
        if(d_samps[i] < minimum) minimum = d_samps[i];
        if(d_samps[i] > maximum) maximum = d_samps[i];
      }
      minimum = get_clean_num(minimum);
      maximum = get_clean_num(maximum);
      if(minimum == maximum || minimum > maximum)
        return; //useless data or screw up?
      /* load the bins */
      int index;
      float bin_width = (maximum - minimum)/(d_num_bins-1);
      for(unsigned int i = 0; i < d_sample_count; i++) {
        index = boost::math::iround((d_samps[i] - minimum)/bin_width);
        /* ensure the index range in case a small floating point error is involed */
        if(index < 0)
          index = 0;
        if(index >= (int)d_num_bins)
          index = d_num_bins-1;
        d_bins[index]++;
      }
      /* Build a message to hold the output records */
      message::sptr msg = message::make(0, minimum, maximum, d_num_bins*sizeof(float));
      float *out = (float *)msg->msg(); // get pointer to raw message buffer
      /* normalize the bins and put into message */
      for(unsigned int i = 0; i < d_num_bins; i++) {
        out[i] = ((float)d_bins[i])/d_frame_size;
      }
      /* send the message */
      d_msgq->handle(msg);
    }

    void
    histo_sink_f_impl::clear(void)
    {
      d_sample_count = 0;
      /* zero the bins */
      for(unsigned int i = 0; i < d_num_bins; i++) {
        d_bins[i] = 0;
      }
    }

    /**************************************************
     * Getters
     **************************************************/
    unsigned int
    histo_sink_f_impl::get_frame_size(void)
    {
      return d_frame_size;
    }

    unsigned int
    histo_sink_f_impl::get_num_bins(void)
    {
      return d_num_bins;
    }

    /**************************************************
     * Setters
     **************************************************/
    void
    histo_sink_f_impl::set_frame_size(unsigned int frame_size)
    {
      gr::thread::scoped_lock guard(d_mutex);    // hold mutex for duration of this function
      d_frame_size = frame_size;
      /* allocate a new sample array */
      delete [] d_samps;
      d_samps = new float[d_frame_size];
      clear();
    }

    void
    histo_sink_f_impl::set_num_bins(unsigned int num_bins) {
      gr::thread::scoped_lock guard(d_mutex);    // hold mutex for duration of this function
      d_num_bins = num_bins;
      /* allocate a new bin array */
      delete [] d_bins;
      d_bins = new unsigned int[d_num_bins];
      clear();
    }

  } /* namespace wxgui */
} /* namespace gr */


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#include "pyinterp/geohash/int64.hpp"

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <Eigen/Core>

#include "pyinterp/geodetic/box.hpp"
#include "pyinterp/geodetic/point.hpp"

namespace py = pybind11;
namespace geohash = pyinterp::geohash;

// Checking the value defining the precision of a geohash.
constexpr static auto check_range(uint32_t precision) -> void {
  if (precision < 1 || precision > 64) {
    throw std::invalid_argument("precision must be within [1, 64]");
  }
}

void init_geohash_int64(py::module& m) {
  m.def(
       "encode",
       [](const Eigen::Ref<const Eigen::VectorXd>& lon,
          const Eigen::Ref<const Eigen::VectorXd>& lat,
          const uint32_t precision) -> pyinterp::Vector<uint64_t> {
         check_range(precision);
         return geohash::int64::encode(lon, lat, precision);
       },
       py::arg("lon"), py::arg("lat"), py::arg("precision") = 64,
       R"__doc__(
Encode coordinates into geohash with the given precision.

Args:
  lon (numpy.ndarray) Longitudes in degrees.
  lat (numpy.ndarray) Latitudes in degrees.
  precision (int, optional) Number of bits used to encode the geohash code.
Returns:
  numpy.ndarray: geohash codes.
Raises:
  ValueError: If the given precision is not within [1, 64].
  ValueError: If the lon and lat vectors have different sizes.
)__doc__")
      .def(
          "decode",
          [](const Eigen::Ref<const pyinterp::Vector<uint64_t>>& hash,
             const uint32_t precision,
             const bool round) -> std::tuple<Eigen::VectorXd, Eigen::VectorXd> {
            check_range(precision);
            return geohash::int64::decode(hash, precision, round);
          },
          py::arg("hash"), py::arg("precision") = 64, py::arg("round") = false,
          R"__doc__(
Decode hash into a geographic points with the given precision.

Args:
  hash (numpy.ndarray): GeoHash.
  precision (int, optional): Required accuracy.
  round (optional, bool): If true, the coordinates of the point will be
    rounded to the accuracy defined by the GeoHash."
Returns:
  tuple: longitudes/latitudes of the decoded points.
Raises:
    ValueError: If the given precision is not within [1, 64].
)__doc__")
      .def(
          "neighbors",
          [](const uint64_t hash,
             const uint32_t precision) -> Eigen::Matrix<uint64_t, 8, 1> {
            check_range(precision);
            return geohash::int64::neighbors(hash, precision);
          },
          py::arg("hash"), py::arg("precision") = 64,
          R"__doc__(
Returns all neighbors hash clockwise from north around northwest
at the given precision.

Args:
  hash (int): Geohash code.
  precision (int, optional): Required accuracy.
Returns:
  numpy.ndarray: Geohash codes.
Raises:
  ValueError: If the given precision is not within [1, 64].
)__doc__");
}


#ifndef HSubproblemDefinitions
#define HSubproblemDefinitions

#include "VectorOperations.hpp"
#include <Eigen/Core>
#include <vector>

template<class TSurrogateModel, template<class TSurrogateModel1> class TSubproblemOptimization>
struct SubproblemData {
    TSubproblemOptimization<TSurrogateModel> *me;
    VectorOperations *vo;
    int constraint_number;
};

template<class TSurrogateModel, template<class TSurrogateModel1> class TSubproblemOptimization>
class SubproblemDefinitions : public VectorOperations {
  public:
    static double opt_trial_point_obj ( std::vector<double> const&, std::vector<double>&, void*);
    static double opt_criticality_measure_obj ( std::vector<double> const &,
                                                std::vector<double>&, void*);
    static double opt_restore_feasibility_obj ( std::vector<double> const &,
                                                std::vector<double>&, void*);
    static double trustregion_constraint ( std::vector<double> const &,
                                     std::vector<double>&, void*);
    static double constraints_for_subproblems ( std::vector<double> const &,
                                          std::vector<double>&, void*);
};

//--------------------------------------------------------------------------------
template<class TSurrogateModel, template<class TSurrogateModel1> class TSubproblemOptimization>
double SubproblemDefinitions<TSurrogateModel, TSubproblemOptimization>::opt_trial_point_obj (
  std::vector<double> const &x, std::vector<double> &grad, void* data)
{
  SubproblemData<TSurrogateModel, TSubproblemOptimization> *d =
    reinterpret_cast<  SubproblemData<TSurrogateModel, TSubproblemOptimization>*>(data);

  if (!grad.empty( )) {
    d->vo->mat_vec_product( (*(d->me->surrogate_models))[0].hessian( ), x, grad );
    d->vo->add ( (*(d->me->surrogate_models))[0].gradient( ), grad );
  }
  return (*(d->me->surrogate_models))[0].evaluate( x );
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
template<class TSurrogateModel, template<class TSurrogateModel1> class TSubproblemOptimization>
double SubproblemDefinitions<TSurrogateModel, TSubproblemOptimization>::opt_criticality_measure_obj (
  std::vector<double> const &x, std::vector<double> &grad, void* data)
{

  SubproblemData<TSurrogateModel, TSubproblemOptimization> *d =
    reinterpret_cast<  SubproblemData<TSurrogateModel, TSubproblemOptimization>*>(data);

  if (!grad.empty( )) {
    grad = d->me->criticality_gradient;
  }

  return d->vo->dot_product( x, d->me->criticality_gradient );
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
template<class TSurrogateModel, template<class TSurrogateModel1> class TSubproblemOptimization>
double SubproblemDefinitions<TSurrogateModel, TSubproblemOptimization>::opt_restore_feasibility_obj (
  std::vector<double> const &x, std::vector<double> &grad, void* data)
{

  SubproblemData<TSurrogateModel, TSubproblemOptimization> *d =
    reinterpret_cast<  SubproblemData<TSurrogateModel, TSubproblemOptimization>*>(data);

//  d->vo->minus( x, d->me->best_point, d->vector );
  std::vector<double> gradient (x.size());
  double objective_value = 0e0;
  double tmp[3];
  double lambda_g = *(d->me->delta);
  if (!grad.empty( )) {
    for (int j = 0; j < x.size( ); ++j)
      grad[ j ] = 0e0;
  }
  tmp[1] = d->vo->dot_product( x, x );
  tmp[2] = pow( *(d->me->delta), 2e0 );

  for ( int i = 0; i < (d->me->surrogate_models)->size()-1; ++i ) {
    tmp[0] = (*(d->me->surrogate_models))[i+1].evaluate( x );
    if ( tmp[0] > 0e0 ) {
      //tmp[0] += d->me->inner_boundary_constant->at(i) * tmp[1] * tmp[2];
      d->vo->mat_vec_product( (*(d->me->surrogate_models))[i+1].hessian( ), x, gradient );
      d->vo->add ((*(d->me->surrogate_models))[i+1].gradient( ), gradient );
      //gradient = (*(d->me->surrogate_models))[i+1].gradient( x );
      objective_value += pow( tmp[0] , 2e0) + lambda_g * tmp[0];
      if (!grad.empty( )) {
        for (int j = 0; j < x.size( ); j++){
          grad[j] += 2e0 * tmp[0] * ( gradient[j] ) + lambda_g * gradient[j];
			// + d->me->inner_boundary_constant->at(i) * x.at(j) * tmp[2]);
      	}
      }
    }
  }

  return objective_value;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
template<class TSurrogateModel, template<class TSurrogateModel1> class TSubproblemOptimization>
double SubproblemDefinitions<TSurrogateModel, TSubproblemOptimization>::constraints_for_subproblems (
  std::vector<double> const &x, std::vector<double> &grad, void* data)
{

  SubproblemData<TSurrogateModel, TSubproblemOptimization> *d =
    reinterpret_cast<  SubproblemData<TSurrogateModel, TSubproblemOptimization>*>(data);

  double result;
  result = (*(d->me->surrogate_models))[d->constraint_number+1].evaluate( x );
  if (!grad.empty()) {
    d->vo->mat_vec_product( (*(d->me->surrogate_models))[d->constraint_number+1].hessian( ), x, grad );
    d->vo->add ( (*(d->me->surrogate_models))[d->constraint_number+1].gradient( ), grad );
  }

  double tmpdbl;
  tmpdbl = d->vo->dot_product( x, x );

  double tmpdbl1 = pow(*(d->me->delta), 2e0);

  result += d->me->inner_boundary_constant->at( d->constraint_number ) * tmpdbl *
            tmpdbl1 -
            d->me->feasibility_thresholds.at( d->constraint_number );
  if (!grad.empty()) {
    tmpdbl = d->me->inner_boundary_constant->at( d->constraint_number ) * 2e0 *
             tmpdbl1;
    for (int j = 0; j < x.size(); ++j)
      grad[j] += tmpdbl * x.at(j);
  }

   return result;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
template<class TSurrogateModel, template<class TSurrogateModel1> class TSubproblemOptimization>
double SubproblemDefinitions<TSurrogateModel, TSubproblemOptimization>::trustregion_constraint (
  std::vector<double> const &x, std::vector<double> &grad, void* data)
{

  SubproblemData<TSurrogateModel, TSubproblemOptimization> *d =
    reinterpret_cast<  SubproblemData<TSurrogateModel, TSubproblemOptimization>*>(data);

  double result;
  result = (d->vo->dot_product( x, x ) - 1e0);

  if (!grad.empty()) {
    for (int i = 0; i < x.size(); ++i)
      grad[i] = 2e0 * x[i];
  }

  return result;
}
//--------------------------------------------------------------------------------

#endif


/*! @file mean.cc
 *  @brief Arithmetic and geometric means calculation.
 *  @author Markovtsev Vadim <v.markovtsev@samsung.com>
 *  @version 1.0
 *
 *  @section Notes
 *  This code partially conforms to Google C++ Style Guide.
 *
 *  @section Copyright
 *  Copyright © 2013 Samsung R&D Institute Russia
 *
 *  @section License
 *  Licensed to the Apache Software Foundation (ASF) under one
 *  or more contributor license agreements.  See the NOTICE file
 *  distributed with this work for additional information
 *  regarding copyright ownership.  The ASF licenses this file
 *  to you under the Apache License, Version 2.0 (the
 *  "License"); you may not use this file except in compliance
 *  with the License.  You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an
 *  "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 *  KIND, either express or implied.  See the License for the
 *  specific language governing permissions and limitations
 *  under the License.
 */

#include "src/transforms/mean.h"
#include <algorithm>
#include <limits>
#include <simd/arithmetic-inl.h>
#include <simd/mathfun.h>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
#include <boost/regex.hpp>
#pragma GCC diagnostic pop

namespace sound_feature_extraction {
namespace transforms {

using formats::FixedArray;

std::set<MeanType> Parse(const std::string& value,
                         identity<std::set<MeanType>>) {
  static const std::unordered_map<std::string, MeanType> map {
    { internal::kMeanTypeArithmeticStr, kMeanTypeArithmetic },
    { internal::kMeanTypeGeometricStr, kMeanTypeGeometric },
  };

  static const boost::regex all_regex("^\\s*(\\w+\\s*(\\s|$))+");
  boost::smatch match;
  if (!boost::regex_match(value, match, all_regex)) {
    throw InvalidParameterValueException();
  }

  std::set<MeanType> ret;
  std::transform(boost::sregex_token_iterator(value.begin(), value.end(),
                                            boost::regex("\\s*(\\w+)\\s*"),
                                            1),
                 boost::sregex_token_iterator(),
                 std::inserter(ret, ret.begin()),
                 [](const std::string& subval) {
    auto mtypeit = map.find(subval);
    if (mtypeit == map.end()) {
      throw InvalidParameterValueException();
    }
    return mtypeit->second;
  });

  return ret;
}

Mean::Mean() : types_(kDefaultMeanTypes()) {
}

ALWAYS_VALID_TP(Mean, types)

void Mean::Do(const float* in,
            FixedArray<kMeanTypeCount>* out) const noexcept {
  for (int j = 0; j < kMeanTypeCount; j++) {
    auto mt = static_cast<MeanType>(j);
    if (types_.find(mt) != types_.end()) {
      (*out)[j] = Do(use_simd(), in, input_format_->Size(), mt);
    } else {
      (*out)[j] = 0;
    }
  }
}

float Mean::Do(bool simd, const float* input, size_t length,
               MeanType type) noexcept {
  int ilength = static_cast<int>(length);
  switch (type) {
    case kMeanTypeArithmetic: {
      float res;
      if (simd) {
#if defined(__AVX__) || defined(__ARM_NEON__)
        res = sum_elements(input, length);
      } else {
#else
      } {
#endif
        res = sum_elements_na(input, length);
      }
      res /= length;
      return res;
    }
    case kMeanTypeGeometric: {
      const float power = 1.f / ilength;
      if (simd) {
#ifdef __AVX__
        __m256 res = _mm256_set1_ps(1.f), tmp = _mm256_set1_ps(1.f);
        const __m256 powvec = _mm256_set1_ps(power);
        const __m256 infvec = _mm256_set1_ps(
            std::numeric_limits<float>::infinity());
        for (int j = 0; j < ilength - 7; j += 8) {
          __m256 vec = _mm256_load_ps(input + j);
          __m256 mulvec = _mm256_mul_ps(tmp, vec);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
          __m256 cmpvec = _mm256_cmp_ps(mulvec, infvec, _CMP_EQ_UQ);
          int check = _mm256_movemask_ps(cmpvec);
          if (check != 0) {
            // Taking a power from 0 can lead to unexpected results...
            // Apply the mask to workaround zeros in tmp
            cmpvec = _mm256_cmp_ps(tmp, _mm256_set1_ps(0.f), _CMP_EQ_UQ);
            tmp = pow256_ps(tmp, powvec);
            tmp = _mm256_blendv_ps(tmp, _mm256_set1_ps(0.f), cmpvec);
            res = _mm256_mul_ps(res, tmp);
            tmp = vec;
          } else {
            tmp = mulvec;
          }
#pragma GCC diagnostic pop
        }
        for (int i = 0; i < 8; i++) {
          if (_mm256_get_ps(tmp, i) == 0) {
            return 0;
          }
        }
        tmp = pow256_ps(tmp, powvec);
        res = _mm256_mul_ps(res, tmp);
        float sctmp = 1.f;
        for (int j = (ilength & ~0x7); j < ilength; j++) {
          sctmp *= input[j];
        }
        float scres = powf(sctmp, power);
        for (int j = 0; j < 8; j++) {
          scres *= _mm256_get_ps(res, j);
        }
        return scres;
      } else {
#elif defined(__ARM_NEON__)
        float32x4_t res = vdupq_n_f32(1.0f), tmp = vdupq_n_f32(1.0f),
            powvec = vdupq_n_f32(power),
            infvec = vdupq_n_f32(std::numeric_limits<float>::infinity());

        for (int j = 0; j < ilength - 3; j += 4) {
          float32x4_t vec = vld1q_f32(input + j);
          float32x4_t mulvec = vmulq_f32(tmp, vec);
          uint32x4_t cmpvec = vceqq_f32(mulvec, infvec);
          uint64x2_t cmpvec2 = vpaddlq_u32(cmpvec);
          if (vgetq_lane_u64(cmpvec2, 0) != 0 ||
              vgetq_lane_u64(cmpvec2, 1) != 0) {
            tmp = pow_ps(tmp, powvec);
            res = vmulq_f32(res, tmp);
            tmp = vec;
          } else {
            tmp = mulvec;
          }
        }
        tmp = pow_ps(tmp, powvec);
        res = vmulq_f32(res, tmp);
        float sctmp = 1.f;
        for (int j = (ilength & ~0x3); j < ilength; j++) {
          sctmp *= input[j];
        }
        float scres = powf(sctmp, power);
        scres *= vgetq_lane_f32(res, 0) * vgetq_lane_f32(res, 1) *
            vgetq_lane_f32(res, 2) * vgetq_lane_f32(res, 3);
        return scres;
      } else {
#else
      } {
#endif
        float res = 1.f, tmp = 1.f;
        for (int j = 0; j < ilength; j++) {
          float val = input[j];
          float multmp = tmp * val;
          if (multmp == std::numeric_limits<float>::infinity()) {
            res *= powf(tmp, power);
            tmp = val;
          } else {
            tmp = multmp;
          }
        }
        if (tmp != 1.f) {
          res *= powf(tmp, power);
        }
        return res;
      }
    }
    default:
      break;
  }
  return 0.f;
}

RTP(Mean, types)
REGISTER_TRANSFORM(Mean);

}  // namespace transforms
}  // namespace sound_feature_extraction


#include "refine.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/index/rtree.hpp>

namespace salt {

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

using BPoint = bg::model::point<DTYPE, 2, bg::cs::cartesian>;
using BSegment = bg::model::segment<BPoint>;
using BBox = bg::model::box<BPoint>;
using RNode = pair<BBox, shared_ptr<TreeNode>>;  // R-Tree node
struct RNodeComp {
    bool operator()(const RNode& l, const RNode& r) const {
        return bg::equals(l.first, r.first) && l.second == r.second;
    }
};

// Remove cases where the intersecion point is two end points
void Purify(const shared_ptr<TreeNode> n, vector<RNode>& cands) {
    vector<RNode> res;
    for (auto c : cands) {
        auto s0 = n->loc;
        auto s1 = n->parent->loc;
        auto c0 = c.second->loc;
        auto c1 = c.second->parent->loc;
        // if (!bg::equals(s0, c0) && !bg::equals(s0, c1) && !bg::equals(s1, c0) && !bg::equals(s1, c1))
        if (s0 != c0 && s0 != c1 && s1 != c0 && s1 != c1) res.push_back(c);
    }
    cands = move(res);
}

// Process the identified intersected edges
void Cancel(array<shared_ptr<TreeNode>, 2> oric) {
    int d;  // direction id: 0-x 1-y

    // make the primary direction for (oric[0] - oric[0]->parent) x
    vector<DTYPE> xys[2];
    for (int d = 0; d < 2; ++d) {
        for (auto sn : {oric[0], oric[1], oric[0]->parent, oric[1]->parent}) xys[d].push_back(sn->loc[d]);
        sort(xys[d].begin(), xys[d].end());
    }
    auto minx = xys[0][0], maxx = xys[0][3], miny = xys[1][0], maxy = xys[1][3];
    if ((minx == oric[1]->loc.x || minx == oric[1]->parent->loc.x) &&
        (maxx == oric[1]->loc.x || maxx == oric[1]->parent->loc.x))  // for corner cases where three is the same
        // || ((miny == oric[0]->loc.y || miny == oric[0]->parent->loc.y) &&
        // (maxy == oric[0]->loc.y || maxy == oric[0]->parent->loc.y)))
        swap(oric[0], oric[1]);

    // make sure oric[d] is in the left side (xys[d][0]) of xys[d]
    for (d = 0; d < 2; ++d) {
        auto v = oric[d]->loc[d];
        if (v != xys[d][0] && v != xys[d][1]) {
            reverse(xys[d].begin(), xys[d].end());
            assert(v == xys[d][0] || v == xys[d][1]);
        }
    }

    // get closest corners in the overlapped box
    Point cclosest[2];
    for (d = 0; d < 2; ++d) {
        // coordinate of secondary direction is the same
        cclosest[d][1 - d] = oric[d]->loc[1 - d];
        // but cannot be extreme
        if (cclosest[d][1 - d] == xys[1 - d][0])
            cclosest[d][1 - d] = xys[1 - d][1];
        else if (cclosest[d][1 - d] == xys[1 - d][3])
            cclosest[d][1 - d] = xys[1 - d][2];
        // coordinate of primary direction is in xys
        cclosest[d][d] = xys[d][1];
        // log() << oric[d]->loc << ": " << cclosest[d] << endl;
    }

    // select oric[minD] (i.e., oric[1-minD]->parent)
    // two choices: 1. select shorter path; 2. select shorter edge
    function<DTYPE(const shared_ptr<TreeNode>&)> DistToSrc = [&](const shared_ptr<TreeNode>& node) -> DTYPE {
        if (!node->parent) return 0;
        return DistToSrc(node->parent) + node->WireToParent();
    };
    DTYPE totDist[2], edgeL[2];
    for (d = 0; d < 2; ++d) {
        edgeL[d] = Dist(oric[d]->parent->loc, cclosest[d]);
        totDist[d] = DistToSrc(oric[d]->parent) + edgeL[d];
    }
    int minD = ((totDist[0] < totDist[1]) || (totDist[0] == totDist[1] && edgeL[0] < edgeL[1])) ? 1 : 0;
    // special case
    auto trace = oric[1 - minD];
    while (trace->parent && trace != oric[minD]) trace = trace->parent;
    if (trace->parent) minD = 1 - minD;

    // clean, create & connect
    auto rootN = oric[1 - minD]->parent;
    shared_ptr<TreeNode> mergeN = nullptr;
    for (int d = 0; d < 2; ++d) {
        TreeNode::ResetParent(oric[d]);
        if (oric[d]->loc == cclosest[d]) mergeN = oric[d];
    }
    if (!mergeN) mergeN = make_shared<TreeNode>(cclosest[minD]);
    TreeNode::SetParent(mergeN, rootN);
    for (int d = 0; d < 2; ++d)
        if (oric[d] != mergeN) TreeNode::SetParent(oric[d], mergeN);
}

void Refine::CancelIntersect(Tree& tree) {
    bgi::rtree<RNode, bgi::linear<16>, bgi::indexable<RNode>, RNodeComp> rtree;
    auto nodes = tree.ObtainNodes();
    for (int i = 0; i < nodes.size(); ++i) {
        auto n = nodes[i];
        if (n->parent) {
            // n->parent & nodes may change
            BBox s;
            while (true) {
                bg::envelope(BSegment(BPoint(n->loc.x, n->loc.y), BPoint(n->parent->loc.x, n->parent->loc.y)), s);
                vector<RNode> cands;
                rtree.query(bgi::intersects(s), back_inserter(cands));
                Purify(n, cands);
                if (cands.empty()) break;
                auto c = cands[0];
                // log() << n->id << "," << n->parent->id << " " << c.second->id << "," << c.second->parent->id << endl;
                Cancel({n, c.second});
                assert(n->parent == c.second->parent || n->parent == c.second || n == c.second->parent);
                rtree.remove(c);
                if (n->parent == c.second->parent) nodes.push_back(n->parent);
                nodes.push_back(c.second);
            }
            rtree.insert({s, n});
        }
    }
    tree.RemoveTopoRedundantSteiner();
}

}  // namespace salt


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/io.h>

#include <iostream>
#include <fstream>

#include <boost/graph/connected_components.hpp>

typedef CGAL::Simple_cartesian<double>                       Kernel;
typedef Kernel::Point_3                                      Point;
typedef CGAL::Surface_mesh<Point>                            Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;

int main()
{
  Mesh sm;
  CGAL::make_quad(Point(0,0,0), Point(1,0,0),Point(1,1,0),Point(0,1,0), sm);
  CGAL::make_quad(Point(0,0,1), Point(1,0,1),Point(1,1,1),Point(0,1,1), sm);

  std::ofstream out("out.inp");
  out.precision(17);
  CGAL::write_inp(out, sm, "out.inp", "S4R");
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <Eigen/Core>
#include <example-robot-data/path.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/parsers/srdf.hpp>
#include <pinocchio/parsers/urdf.hpp>

#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/integrator/euler.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/core/solvers/ddp.hpp"
#include "crocoddyl/core/utils/callbacks.hpp"
#include "crocoddyl/core/utils/timer.hpp"
#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

using boost::make_shared;
using boost::shared_ptr;
using namespace crocoddyl;

std::tuple<shared_ptr<ActionModelAbstractTpl<double>>, shared_ptr<ActionModelAbstractTpl<double>>>
build_arm_action_models()
{
    typedef typename MathBaseTpl<double>::Vector3s Vector3s;
    typedef typename MathBaseTpl<double>::Matrix3s Matrix3s;

    // because urdf is not supported with all double types.
    pinocchio::ModelTpl<double> modeld;
    pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_left_arm.urdf", modeld);
    pinocchio::srdf::loadReferenceConfigurations(modeld, EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf",
                                                 false);

    pinocchio::ModelTpl<double> model_full(modeld.cast<double>()), model;
    std::vector<pinocchio::JointIndex> locked_joints;
    locked_joints.push_back(5);
    locked_joints.push_back(6);
    locked_joints.push_back(7);
    pinocchio::buildReducedModel(model_full, locked_joints, Eigen::VectorXd::Zero(model_full.nq), model);

    shared_ptr<StateMultibodyTpl<double>> state =
        make_shared<StateMultibodyTpl<double>>(make_shared<pinocchio::ModelTpl<double>>(model));

    auto goalTrackingCost = make_shared<CostModelResidualTpl<double>>(
        state, make_shared<ResidualModelFramePlacementTpl<double>>(
                   state, model.getFrameId("gripper_left_joint"),
                   pinocchio::SE3Tpl<double>(Matrix3s::Identity(), Vector3s(double(0), double(0), double(.4)))));
    auto xRegCost = make_shared<CostModelResidualTpl<double>>(state, make_shared<ResidualModelStateTpl<double>>(state));
    auto uRegCost =
        make_shared<CostModelResidualTpl<double>>(state, make_shared<ResidualModelControlTpl<double>>(state));

    // Create a cost model per the running and terminal action model.
    auto runningCostModel = make_shared<CostModelSumTpl<double>>(state);
    auto terminalCostModel = make_shared<CostModelSumTpl<double>>(state);

    // Then let's added the running and terminal cost functions
    runningCostModel->addCost("gripperPose", goalTrackingCost, double(1));
    runningCostModel->addCost("xReg", xRegCost, double(1e-4));
    runningCostModel->addCost("uReg", uRegCost, double(1e-4));
    terminalCostModel->addCost("gripperPose", goalTrackingCost, double(1));

    // We define an actuation model
    auto actuation = make_shared<ActuationModelFullTpl<double>>(state);

    // Next, we need to create an action model for running and terminal knots. The
    // forward dynamics (computed using ABA) are implemented
    // inside DifferentialActionModelFullyActuated.
    auto runningDAM =
        make_shared<DifferentialActionModelFreeFwdDynamicsTpl<double>>(state, actuation, runningCostModel);

    auto runningModel = make_shared<IntegratedActionModelEulerTpl<double>>(runningDAM, double(1e-3));
    auto terminalModel = make_shared<IntegratedActionModelEulerTpl<double>>(runningDAM, double(0.));

    return {runningModel, terminalModel};
}

int main(int argc, char *argv[])
{
    unsigned int N = 100;  // number of nodes
    unsigned int T = 5e3;  // number of trials
    unsigned int MAXITER = 1;

    if (argc > 1)
    {
        T = atoi(argv[1]);
    }

    // Building the running and terminal models
    auto [runningModel, terminalModel] = build_arm_action_models();

    // Get the initial state
    shared_ptr<StateMultibody> state = boost::static_pointer_cast<StateMultibody>(runningModel->get_state());

    std::cout << "NQ: " << state->get_nq() << std::endl;
    std::cout << "Number of nodes: " << N << std::endl << std::endl;

    Eigen::VectorXd q0 = Eigen::VectorXd::Random(state->get_nq());
    Eigen::VectorXd x0(state->get_nx());
    x0 << q0, Eigen::VectorXd::Random(state->get_nv());

    // For this optimal control problem, we define 100 knots (or running action
    // models) plus a terminal knot
    std::vector<shared_ptr<ActionModelAbstract>> runningModels(N, runningModel);
    ShootingProblem problem(x0, runningModels, terminalModel);
    std::vector<Eigen::VectorXd> xs(N + 1, x0);
    std::vector<Eigen::VectorXd> us(N, Eigen::VectorXd::Zero(runningModel->get_nu()));
    for (unsigned int i = 0; i < N; ++i)
    {
        const shared_ptr<ActionModelAbstract> &model = problem.get_runningModels()[i];
        const shared_ptr<ActionDataAbstract> &data = problem.get_runningDatas()[i];
        model->quasiStatic(data, us[i], x0);
    }

    // Formulating the optimal control problem
    SolverDDP ddp(problem);

    // Solving the optimal control problem
    Eigen::ArrayXd duration(T);
    for (unsigned int i = 0; i < T; ++i)
    {
        Timer timer;
        ddp.solve(xs, us, MAXITER, false, 0.1);
        duration[i] = timer.get_duration();
    }

    double avrg_duration = duration.sum() / T;
    double min_duration = duration.minCoeff();
    double max_duration = duration.maxCoeff();
    std::cout << "  DDP.solve [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
              << std::endl;

    // Running calc
    for (unsigned int i = 0; i < T; ++i)
    {
        Timer timer;
        problem.calc(xs, us);
        duration[i] = timer.get_duration();
    }

    avrg_duration = duration.sum() / T;
    min_duration = duration.minCoeff();
    max_duration = duration.maxCoeff();
    std::cout << "  ShootingProblem.calc [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
              << std::endl;

    // Running calcDiff
    for (unsigned int i = 0; i < T; ++i)
    {
        Timer timer;
        problem.calcDiff(xs, us);
        duration[i] = timer.get_duration();
    }

    avrg_duration = duration.sum() / T;
    min_duration = duration.minCoeff();
    max_duration = duration.maxCoeff();
    std::cout << "  ShootingProblem.calcDiff [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration
              << ")" << std::endl;
}

// import sys

// import crocoddyl
// import numpy as np
// import example_robot_data
// import pinocchio

// # Load robot
// robot = example_robot_data.load("talos")

// # Create data structures
// rdata = robot.model.createData()
// state = crocoddyl.StateMultibody(robot.model)
// actuation = crocoddyl.ActuationModelFloatingBase(state)

// # Set integration time
// DT = 5e-2
// T = 60
// target = np.array([0.5, 0, 1.8])

// # Initialize reference state, target and reference CoM
// rightFoot = "right_sole_link"
// leftFoot = "left_sole_link"
// endEffector = "gripper_left_joint"
// endEffectorId = robot.model.getFrameId(endEffector)
// rightFootId = robot.model.getFrameId(rightFoot)
// leftFootId = robot.model.getFrameId(leftFoot)
// q0 = robot.model.referenceConfigurations["half_sitting"]
// x0 = np.concatenate([q0, np.zeros(robot.model.nv)])
// pinocchio.forwardKinematics(robot.model, rdata, q0)
// pinocchio.updateFramePlacements(robot.model, rdata)

// # Initialize Gepetto viewer
// display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
// display.robot.viewer.gui.addSphere("world/point", 0.05, [1.0, 0.0, 0.0, 1.0])
// display.robot.viewer.gui.applyConfiguration("world/point", target.tolist() + [0.0, 0.0, 0.0, 1.0])

// # Add contact to the model
// contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu)
// supportContactModelLeft = crocoddyl.ContactModel6D(
//     state, leftFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
// )
// contactModel.addContact(leftFoot + "_contact", supportContactModelLeft)
// supportContactModelRight = crocoddyl.ContactModel6D(
//     state, rightFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
// )
// contactModel.addContact(rightFoot + "_contact", supportContactModelRight)

// contactModelLeft = crocoddyl.ContactModelMultiple(state, actuation.nu)
// contactModelLeft.addContact(
//     "contact",
//     crocoddyl.ContactModel6D(
//         state, leftFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
//     ),
// )

// contactModelRight = crocoddyl.ContactModelMultiple(state, actuation.nu)
// contactModelRight.addContact(
//     "contact",
//     crocoddyl.ContactModel6D(
//         state, rightFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])
//     ),
// )

// # Cost for self-collision
// maxfloat = sys.float_info.max
// xlb = np.concatenate(
//     [
//         -maxfloat * np.ones(6),  # dimension of the SE(3) manifold
//         robot.model.lowerPositionLimit[7:],
//         -maxfloat * np.ones(state.nv),
//     ]
// )
// xub = np.concatenate(
//     [
//         maxfloat * np.ones(6),  # dimension of the SE(3) manifold
//         robot.model.upperPositionLimit[7:],
//         maxfloat * np.ones(state.nv),
//     ]
// )
// bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
// xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
// xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
// limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)

// # Cost for state and control
// xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
// xActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) ** 2
// )
// uResidual = crocoddyl.ResidualModelControl(state, actuation.nu)
// xTActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) ** 2
// )
// xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
// uRegCost = crocoddyl.CostModelResidual(state, uResidual)
// xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)

// # Cost for target reaching
// framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
//     state, endEffectorId, pinocchio.SE3(np.eye(3), target), actuation.nu
// )
// framePlacementActivation = crocoddyl.ActivationModelWeightedQuad(
//     np.array([1] * 3 + [0.0001] * 3) ** 2
// )
// goalTrackingCost = crocoddyl.CostModelResidual(
//     state, framePlacementActivation, framePlacementResidual
// )

// # Create cost model per each action model
// runningCostModel = crocoddyl.CostModelSum(state, actuation.nu)
// terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)

// # Then let's added the running and terminal cost functions
// runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
// runningCostModel.addCost("stateReg", xRegCost, 1e-3)
// runningCostModel.addCost("ctrlReg", uRegCost, 1e-4)
// runningCostModel.addCost("limitCost", limitCost, 1e3)

// terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
// terminalCostModel.addCost("stateReg", xRegTermCost, 1e-3)
// terminalCostModel.addCost("limitCost", limitCost, 1e3)

// # Create the action model
// dmodelRunningLeft = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelLeft, runningCostModel
// )
// dmodelTerminalLeft = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelLeft, terminalCostModel
// )

// dmodelRunning = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelRight, runningCostModel
// )
// dmodelTerminal = crocoddyl.DifferentialActionModelContactFwdDynamics(
//     state, actuation, contactModelRight, terminalCostModel
// )
// runningModelLeft = crocoddyl.IntegratedActionModelEuler(dmodelRunningLeft, DT)
// runningModel = crocoddyl.IntegratedActionModelEuler(dmodelRunning, DT)
// terminalModel = crocoddyl.IntegratedActionModelEuler(dmodelTerminal, 0)

// # Problem definition
// x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
// problem = crocoddyl.ShootingProblem(x0, [runningModel] * T + [runningModelLeft] * T, terminalModel)

// # Creating the DDP solver for this OC problem, defining a logger
// solver = crocoddyl.SolverFDDP(problem)
// solver.setCallbacks(
//     [
//         crocoddyl.CallbackVerbose(),
//         crocoddyl.CallbackDisplay(
//             crocoddyl.GepettoDisplay(robot, 4, 4, frameNames=[rightFoot, leftFoot])
//         ),
//     ]
// )

// # Solving it with the FDDP algorithm
// xs = [x0] * (solver.problem.T + 1)
// us = solver.problem.quasiStatic([x0] * solver.problem.T)
// solver.solve(xs, us, 500, False, 0.1)

// # Visualizing the solution in gepetto-viewer
// display.displayFromSolver(solver)

// # Get final state and end effector position
// xT = solver.xs[-1]
// pinocchio.forwardKinematics(robot.model, rdata, xT[: state.nq])
// pinocchio.updateFramePlacements(robot.model, rdata)
// com = pinocchio.centerOfMass(robot.model, rdata, xT[: state.nq])
// finalPosEff = np.array(rdata.oMf[robot.model.getFrameId("gripper_left_joint")].translation.T.flat)

// print("Finally reached = ", finalPosEff)
// print("Distance between hand and target = ", np.linalg.norm(finalPosEff - target))
// print("Distance to default state = ", np.linalg.norm(x0 - np.array(xT.flat)))
// # print("XY distance to CoM reference = ", np.linalg.norm(com[:2] - comRef[:2]))


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.

#ifndef poplibs_test_GeneralMatrixMultiply_hpp
#define poplibs_test_GeneralMatrixMultiply_hpp

#include <boost/multi_array.hpp>

namespace poplibs_test {
namespace gemm {

/*
 * Computes matC = alpha * op(matA) .* op(matB)
 *
 * where .* is an element-wise multiplication
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 *
 *       op(matB) = B     if transposeB = false
 *       op(matB) = B'    if transposeB = true
 *
 * Matrix dimensions of op(A) must be equal to op(B)
 */
void hadamardProduct(const boost::multi_array_ref<double, 2> matA,
                     const boost::multi_array_ref<double, 2> matB,
                     boost::multi_array_ref<double, 2> matC, float alpha = 1.0,
                     bool transposeA = false, bool transposeB = false);

/*
 * Computes matC = alpha * op(vecA) .* op(vecB)
 *
 * where .* is an element-wise multiplication
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 *
 *       op(matB) = B     if transposeB = false
 *       op(matB) = B'    if transposeB = true
 *
 * Vector dimensions of A must be equal to B
 */
void hadamardProduct(const boost::multi_array_ref<double, 1> matA,
                     const boost::multi_array_ref<double, 1> matB,
                     boost::multi_array_ref<double, 1> matC, float alpha = 1.0);

/*
 * Computes matD = beta * matC + alpha * op(matA) * matB
 *
 * where matB, matC and matD are one dimensional matrices
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 */
void generalMatrixMultiply(const boost::multi_array_ref<double, 2> matA,
                           const boost::multi_array_ref<double, 1> vecB,
                           const boost::multi_array_ref<double, 1> vecC,
                           boost::multi_array_ref<double, 1> vecD,
                           float alpha = 1.0, float beta = 1.0,
                           bool transposeA = false);

/*
 * Computes matD = beta * matC + alpha * op(matA) * op(matB)
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 *
 *       op(matB) = B     if transposeB = false
 *       op(matB) = B'    if transposeB = true
 *
 */
void generalMatrixMultiply(const boost::multi_array_ref<double, 2> matA,
                           const boost::multi_array_ref<double, 2> matB,
                           const boost::multi_array_ref<double, 2> matC,
                           boost::multi_array_ref<double, 2> matD,
                           float alpha = 1.0, float beta = 1.0,
                           bool transposeA = false, bool transposeB = false);

/*
 * Computes matD = beta * matC + alpha * op(matA) * op(matB) for each matrix in
 * the group. The first dimension is the number of groups and should be the same
 * for matA, matB and matC.
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 *
 *       op(matB) = B     if transposeB = false
 *       op(matB) = B'    if transposeB = true
 *
 */
void generalGroupedMatrixMultiply(const boost::multi_array_ref<double, 3> matA,
                                  const boost::multi_array_ref<double, 3> matB,
                                  const boost::multi_array_ref<double, 3> matC,
                                  boost::multi_array_ref<double, 3> matD,
                                  float alpha = 1.0, float beta = 1.0,
                                  bool transposeA = false,
                                  bool transposeB = false);

/*
 * Computes matC = op(matA) * op(matB)
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 *
 *       op(matB) = B     if transposeB = false
 *       op(matB) = B'    if transposeB = true
 *
 */
void generalMatrixMultiply(const boost::multi_array_ref<double, 2> matA,
                           const boost::multi_array_ref<double, 2> matB,
                           boost::multi_array_ref<double, 2> matC,
                           bool transposeA = false, bool transposeB = false);

/*
 * Computes matC = op(matA) * op(matB) for each matrix in
 * the group. The first dimension is the number of groups and should be the same
 * for matA, matB and matC.
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 *
 *       op(matB) = B     if transposeB = false
 *       op(matB) = B'    if transposeB = true
 *
 */
void generalGroupedMatrixMultiply(const boost::multi_array_ref<double, 3> matA,
                                  const boost::multi_array_ref<double, 3> matB,
                                  boost::multi_array_ref<double, 3> matC,
                                  bool transposeA = false,
                                  bool transposeB = false);

} // End namespace gemm.
} // namespace poplibs_test

#endif // poplibs_test_GeneralMatrixMultiply_hpp


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

#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

#include <opengv/absolute_pose/modules/main.hpp>
#include <opengv/absolute_pose/modules/gp3p/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp1/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp2/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp3/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp4/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp5/modules.hpp>
#include <opengv/absolute_pose/modules/upnp2.hpp>
#include <opengv/absolute_pose/modules/upnp4.hpp>
#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/roots.hpp>
#include <opengv/math/arun.hpp>
#include <opengv/math/cayley.hpp>

void
opengv::absolute_pose::modules::p3p_kneip_main(
    const bearingVectors_t & f,
    const points_t & p,
    transformations_t & solutions )
{
  point_t P1 = p[0];
  point_t P2 = p[1];
  point_t P3 = p[2];

  Eigen::Vector3d temp1 = P2 - P1;
  Eigen::Vector3d temp2 = P3 - P1;

  if( temp1.cross(temp2).norm() == 0)
    return;

  bearingVector_t f1 = f[0];
  bearingVector_t f2 = f[1];
  bearingVector_t f3 = f[2];

  Eigen::Vector3d e1 = f1;
  Eigen::Vector3d e3 = f1.cross(f2);
  e3 = e3/e3.norm();
  Eigen::Vector3d e2 = e3.cross(e1);

  rotation_t T;
  T.row(0) = e1.transpose();
  T.row(1) = e2.transpose();
  T.row(2) = e3.transpose();

  f3 = T*f3;

  if( f3(2,0) > 0)
  {
    f1 = f[1];
    f2 = f[0];
    f3 = f[2];

    e1 = f1;
    e3 = f1.cross(f2);
    e3 = e3/e3.norm();
    e2 = e3.cross(e1);

    T.row(0) = e1.transpose();
    T.row(1) = e2.transpose();
    T.row(2) = e3.transpose();

    f3 = T*f3;

    P1 = p[1];
    P2 = p[0];
    P3 = p[2];
  }

  Eigen::Vector3d n1 = P2-P1;
  n1 = n1/n1.norm();
  Eigen::Vector3d n3 = n1.cross(P3-P1);
  n3 = n3/n3.norm();
  Eigen::Vector3d n2 = n3.cross(n1);

  rotation_t N;
  N.row(0) = n1.transpose();
  N.row(1) = n2.transpose();
  N.row(2) = n3.transpose();

  P3 = N*(P3-P1);

  double d_12 = temp1.norm();
  double f_1 = f3(0,0)/f3(2,0);
  double f_2 = f3(1,0)/f3(2,0);
  double p_1 = P3(0,0);
  double p_2 = P3(1,0);

  double cos_beta = f1.dot(f2);
  double b = 1/( 1 - pow( cos_beta, 2 ) ) - 1;

  if( cos_beta < 0 )
    b = -sqrt(b);
  else
    b = sqrt(b);

  double f_1_pw2 = pow(f_1,2);
  double f_2_pw2 = pow(f_2,2);
  double p_1_pw2 = pow(p_1,2);
  double p_1_pw3 = p_1_pw2 * p_1;
  double p_1_pw4 = p_1_pw3 * p_1;
  double p_2_pw2 = pow(p_2,2);
  double p_2_pw3 = p_2_pw2 * p_2;
  double p_2_pw4 = p_2_pw3 * p_2;
  double d_12_pw2 = pow(d_12,2);
  double b_pw2 = pow(b,2);

  Eigen::Matrix<double,5,1> factors;

  factors(0,0) = -f_2_pw2*p_2_pw4
                 -p_2_pw4*f_1_pw2
                 -p_2_pw4;

  factors(1,0) = 2*p_2_pw3*d_12*b
                 +2*f_2_pw2*p_2_pw3*d_12*b
                 -2*f_2*p_2_pw3*f_1*d_12;

  factors(2,0) = -f_2_pw2*p_2_pw2*p_1_pw2
                 -f_2_pw2*p_2_pw2*d_12_pw2*b_pw2
                 -f_2_pw2*p_2_pw2*d_12_pw2
                 +f_2_pw2*p_2_pw4
                 +p_2_pw4*f_1_pw2
                 +2*p_1*p_2_pw2*d_12
                 +2*f_1*f_2*p_1*p_2_pw2*d_12*b
                 -p_2_pw2*p_1_pw2*f_1_pw2
                 +2*p_1*p_2_pw2*f_2_pw2*d_12
                 -p_2_pw2*d_12_pw2*b_pw2
                 -2*p_1_pw2*p_2_pw2;

  factors(3,0) = 2*p_1_pw2*p_2*d_12*b
                 +2*f_2*p_2_pw3*f_1*d_12
                 -2*f_2_pw2*p_2_pw3*d_12*b
                 -2*p_1*p_2*d_12_pw2*b;

  factors(4,0) = -2*f_2*p_2_pw2*f_1*p_1*d_12*b
                 +f_2_pw2*p_2_pw2*d_12_pw2
                 +2*p_1_pw3*d_12
                 -p_1_pw2*d_12_pw2
                 +f_2_pw2*p_2_pw2*p_1_pw2
                 -p_1_pw4
                 -2*f_2_pw2*p_2_pw2*p_1*d_12
                 +p_2_pw2*f_1_pw2*p_1_pw2
                 +f_2_pw2*p_2_pw2*d_12_pw2*b_pw2;

  std::vector<double> realRoots = math::o4_roots(factors);

  for( int i = 0; i < 4; i++ )
  {
    double cot_alpha =
        (-f_1*p_1/f_2-realRoots[i]*p_2+d_12*b)/
        (-f_1*realRoots[i]*p_2/f_2+p_1-d_12);

    double cos_theta = realRoots[i];
    double sin_theta = sqrt(1-pow(realRoots[i],2));
    double sin_alpha = sqrt(1/(pow(cot_alpha,2)+1));
    double cos_alpha = sqrt(1-pow(sin_alpha,2));

    if (cot_alpha < 0)
      cos_alpha = -cos_alpha;

    translation_t C;
    C(0,0) = d_12*cos_alpha*(sin_alpha*b+cos_alpha);
    C(1,0) = cos_theta*d_12*sin_alpha*(sin_alpha*b+cos_alpha);
    C(2,0) = sin_theta*d_12*sin_alpha*(sin_alpha*b+cos_alpha);

    C = P1 + N.transpose()*C;

    rotation_t R;
    R(0,0) = -cos_alpha;
    R(0,1) = -sin_alpha*cos_theta;
    R(0,2) = -sin_alpha*sin_theta;
    R(1,0) = sin_alpha;
    R(1,1) = -cos_alpha*cos_theta;
    R(1,2) = -cos_alpha*sin_theta;
    R(2,0) = 0.0;
    R(2,1) = -sin_theta;
    R(2,2) = cos_theta;

    R = N.transpose()*R.transpose()*T;

    transformation_t solution;
    solution.col(3) = C;
    solution.block<3,3>(0,0) = R;

    solutions.push_back(solution);
  }
}

void
opengv::absolute_pose::modules::p3p_gao_main(
    const bearingVectors_t & f,
    const points_t & points,
    transformations_t & solutions )
{
  point_t A = points[0];
  point_t B = points[1];
  point_t C = points[2];

  Eigen::Vector3d tempp;
  tempp = A-B;
  double AB = tempp.norm();
  tempp = B-C;
  double BC = tempp.norm();
  tempp = A-C;
  double AC = tempp.norm();

  bearingVector_t f1 = f[0];
  bearingVector_t f2 = f[1];
  bearingVector_t f3 = f[2];

  double cosalpha = f2.transpose()*f3;
  double cosbeta = f1.transpose()*f3;
  double cosgamma = f1.transpose()*f2;

  double a=pow((BC/AB),2);
  double b=pow((AC/AB),2);
  double p=2*cosalpha;
  double q=2*cosbeta;
  double r=2*cosgamma;

  double aSq = a * a;
  double bSq = b * b;
  double pSq = p*p;
  double qSq = q*q;
  double rSq = r*r;

  if ((pSq + qSq + rSq - p*q*r - 1) == 0)
    return;

  Eigen::Matrix<double,5,1> factors;

  factors[0] = -2*b + bSq + aSq + 1 - b*rSq*a + 2*b*a - 2*a;

  if (factors[0] == 0)
    return;

  factors[1] =
      -2*b*q*a - 2*aSq*q + b*rSq*q*a - 2*q + 2*b*q +
      4*a*q + p*b*r + b*r*p*a - bSq*r*p;
  factors[2] =
      qSq + bSq*rSq - b*pSq - q*p*b*r + bSq*pSq - b*rSq*a +
      2 - 2*bSq - a*b*r*p*q + 2*aSq - 4*a - 2*qSq*a + qSq*aSq;
  factors[3] =
      -bSq*r*p + b*r*p*a - 2*aSq*q + q*pSq*b +
      2*b*q*a + 4*a*q + p*b*r - 2*b*q - 2*q;
  factors[4] = 1 - 2*a + 2*b + bSq - b*pSq + aSq - 2*b*a;

  std::vector<double> x_temp = math::o4_roots(factors);
  Eigen::Matrix<double,4,1> x;
  for( size_t i = 0; i < 4; i++ ) x[i] = x_temp[i];

  double temp = (pSq*(a-1+b) + p*q*r - q*a*r*p + (a-1-b)*rSq);
  double b0 = b * temp * temp;

  double rCb = rSq*r;

  Eigen::Matrix<double,4,1> tempXP2;
  tempXP2[0] = x[0]*x[0];
  tempXP2[1] = x[1]*x[1];
  tempXP2[2] = x[2]*x[2];
  tempXP2[3] = x[3]*x[3];
  Eigen::Matrix<double,4,1> tempXP3;
  tempXP3[0] = tempXP2[0]*x[0];
  tempXP3[1] = tempXP2[1]*x[1];
  tempXP3[2] = tempXP2[2]*x[2];
  tempXP3[3] = tempXP2[3]*x[3];

  Eigen::Matrix<double,4,1> ones;
  for( size_t i = 0; i < 4; i++) ones[i] = 1.0;

  Eigen::Matrix<double,4,1> b1_part1 =
      (1-a-b)*tempXP2 + (q*a-q)*x + (1 - a + b)*ones;

  Eigen::Matrix<double,4,1> b1_part2 =
      (aSq*rCb + 2*b*rCb*a - b*rSq*rCb*a - 2*a*rCb + rCb + bSq*rCb
      - 2*rCb*b)*tempXP3
      +(p*rSq + p*aSq*rSq - 2*b*rCb*q*a + 2*rCb*b*q - 2*rCb*q - 2*p*(a+b)*rSq
      + rSq*rSq*p*b + 4*a*rCb*q + b*q*a*rCb*rSq - 2*rCb*aSq*q +2*rSq*p*b*a
      + bSq*rSq*p - rSq*rSq*p*bSq)*tempXP2
      +(rCb*qSq + rSq*rCb*bSq + r*pSq*bSq - 4*a*rCb - 2*a*rCb*qSq + rCb*qSq*aSq
      + 2*aSq*rCb - 2*bSq*rCb - 2*pSq*b*r + 4*p*a*rSq*q + 2*a*pSq*r*b
      - 2*a*rSq*q*b*p - 2*pSq*a*r + r*pSq - b*rSq*rCb*a + 2*p*rSq*b*q
      + r*pSq*aSq -2*p*q*rSq + 2*rCb - 2*rSq*p*aSq*q - rSq*rSq*q*b*p)*x
      +(4*a*rCb*q + p*rSq*qSq + 2*pSq*p*b*a - 4*p*a*rSq - 2*rCb*b*q - 2*pSq*q*r
      - 2*bSq*rSq*p + rSq*rSq*p*b + 2*p*aSq*rSq - 2*rCb*aSq*q - 2*pSq*p*a
      + pSq*p*aSq + 2*p*rSq + pSq*p + 2*b*rCb*q*a + 2*q*pSq*b*r + 4*q*a*r*pSq
      - 2*p*a*rSq*qSq - 2*pSq*aSq*r*q + p*aSq*rSq*qSq - 2*rCb*q - 2*pSq*p*b
      + pSq*p*bSq - 2*pSq*b*r*q*a)*ones;

  Eigen::Matrix<double,4,1> b1;
  b1[0] = b1_part1[0]*b1_part2[0];
  b1[1] = b1_part1[1]*b1_part2[1];
  b1[2] = b1_part1[2]*b1_part2[2];
  b1[3] = b1_part1[3]*b1_part2[3];

  Eigen::Matrix<double,4,1> y=b1/b0;
  Eigen::Matrix<double,4,1> tempYP2;
  tempYP2[0] = pow(y[0],2);
  tempYP2[1] = pow(y[1],2);
  tempYP2[2] = pow(y[2],2);
  tempYP2[3] = pow(y[3],2);

  Eigen::Matrix<double,4,1> tempXY;
  tempXY[0] = x[0]*y[0];
  tempXY[1] = x[1]*y[1];
  tempXY[2] = x[2]*y[2];
  tempXY[3] = x[3]*y[3];

  Eigen::Matrix<double,4,1> v= tempXP2 + tempYP2 - r*tempXY;

  Eigen::Matrix<double,4,1> Z;
  Z[0] = AB/sqrt(v[0]);
  Z[1] = AB/sqrt(v[1]);
  Z[2] = AB/sqrt(v[2]);
  Z[3] = AB/sqrt(v[3]);

  Eigen::Matrix<double,4,1> X;
  X[0] = x[0]*Z[0];
  X[1] = x[1]*Z[1];
  X[2] = x[2]*Z[2];
  X[3] = x[3]*Z[3];

  Eigen::Matrix<double,4,1> Y;
  Y[0] = y[0]*Z[0];
  Y[1] = y[1]*Z[1];
  Y[2] = y[2]*Z[2];
  Y[3] = y[3]*Z[3];

  for( int i = 0; i < 4; i++ )
  {
    //apply arun to find the transformation
    points_t p_cam;
    p_cam.push_back(X[i]*f1);
    p_cam.push_back(Y[i]*f2);
    p_cam.push_back(Z[i]*f3);

    transformation_t solution = math::arun_complete(points,p_cam);
    solutions.push_back(solution);
  }
}

void
opengv::absolute_pose::modules::gp3p_main(
    const Eigen::Matrix3d & f,
    const Eigen::Matrix3d & v,
    const Eigen::Matrix3d & p,
    transformations_t & solutions)
{
  Eigen::Matrix<double,48,85> groebnerMatrix =
      Eigen::Matrix<double,48,85>::Zero();
  gp3p::init(groebnerMatrix,f,v,p);
  gp3p::compute(groebnerMatrix);

  Eigen::Matrix<double,8,8> M = Eigen::Matrix<double,8,8>::Zero();
  M.block<6,8>(0,0) = -groebnerMatrix.block<6,8>(36,77);
  M(6,0) = 1.0;
  M(7,6) = 1.0;

  Eigen::ComplexEigenSolver< Eigen::Matrix<double,8,8> > Eig(M,true);
  Eigen::Matrix<std::complex<double>,8,1> D = Eig.eigenvalues();
  Eigen::Matrix<std::complex<double>,8,8> V = Eig.eigenvectors();

  for( int c = 0; c < V.cols(); c++ )
  {
    std::complex<double> eigValue = D[c];

    if( eigValue.imag() < 0.0001 )
    {
      cayley_t cayley;
      Eigen::Vector3d n;

      for(size_t i = 0; i < 3; i++)
      {
        std::complex<double> cay = V(i+4,c)/V(7,c);
        cayley[2-i] = cay.real();
        std::complex<double> depth = V(i+1,c)/V(7,c);
        n[2-i] = depth.real();
      }

      rotation_t rotation = math::cayley2rot(cayley);
      //the groebner problem was set up to find the transpose!
      rotation.transposeInPlace();

      point_t center_cam = Eigen::Vector3d::Zero();
      point_t center_world = Eigen::Vector3d::Zero();
      for( size_t i = 0; i < (size_t) f.cols(); i++ )
      {
        point_t temp = rotation*(n[i]*f.col(i)+v.col(i));
        center_cam = center_cam + temp;
        center_world = center_world + p.col(i);
      }

      center_cam = center_cam/f.cols();
      center_world = center_world/f.cols();
      translation_t translation = center_world - center_cam;

      transformation_t transformation;
      transformation.block<3,3>(0,0) = rotation;
      transformation.col(3) = translation;
      solutions.push_back(transformation);
    }
  }
}

void
opengv::absolute_pose::modules::gpnp_main(
    const Eigen::Matrix<double,12,1> & a,
    const Eigen::Matrix<double,12,12> & V,
    const points_t & c,
    transformation_t & transformation )
{
  //extracting the nullspace vectors
  Eigen::Matrix<double,12,1> vec_5 = V.col(7);
  Eigen::Matrix<double,12,1> vec_4 = V.col(8);
  Eigen::Matrix<double,12,1> vec_3 = V.col(9);
  Eigen::Matrix<double,12,1> vec_2 = V.col(10);
  Eigen::Matrix<double,12,1> vec_1 = V.col(11);

  point_t c0 = c[0];
  point_t c1 = c[0];
  point_t c2 = c[0];
  point_t c3 = c[0];

  Eigen::Matrix<double,12,1> solution;
  std::vector<double> errors;
  translation_t t;
  translations_t ts;
  rotation_t R;
  rotations_t Rs;
  std::vector<double> factors;

  solution = a;
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //nice, now we just need to find the right combination
  //let's start with trying out the linear combination of the most right
  //null-space vector
  Eigen::Matrix<double,5,3> groebnerMatrix1 =
      Eigen::Matrix<double,5,3>::Zero();
  gpnp1::init(groebnerMatrix1,a,vec_1,c0,c1,c2,c3);
  gpnp1::compute(groebnerMatrix1);
  factors.push_back(-groebnerMatrix1(3,2)/groebnerMatrix1(3,1));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using two nullspace vectors
  Eigen::Matrix<double,10,6> groebnerMatrix2 =
      Eigen::Matrix<double,10,6>::Zero();
  gpnp2::init(groebnerMatrix2,a,vec_2,vec_1,c0,c1,c2,c3);
  gpnp2::compute(groebnerMatrix2);
  factors[0] = -groebnerMatrix2(8,5)/groebnerMatrix2(8,4);
  factors.push_back(
      -(groebnerMatrix2(7,4)*factors[0]+groebnerMatrix2(7,5))/
      groebnerMatrix2(7,3));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using three nullspace vectors
  Eigen::Matrix<double,15,18> groebnerMatrix3 =
      Eigen::Matrix<double,15,18>::Zero();
  gpnp3::init(groebnerMatrix3,a,vec_3,vec_2,vec_1,c0,c1,c2,c3);
  gpnp3::compute(groebnerMatrix3);
  factors[0] = -groebnerMatrix3(13,17)/groebnerMatrix3(13,16);
  factors[1] =
      -(groebnerMatrix3(12,16)*factors[0]+groebnerMatrix3(12,17))/
      groebnerMatrix3(12,15);
  factors.push_back(
      -(groebnerMatrix3(11,15)*factors[1]+groebnerMatrix3(11,16)*factors[0]+
      groebnerMatrix3(11,17))/groebnerMatrix3(11,14));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using four nullspace vectors
  Eigen::Matrix<double,25,37> groebnerMatrix4 =
      Eigen::Matrix<double,25,37>::Zero();
  gpnp4::init(groebnerMatrix4,a,vec_4,vec_3,vec_2,vec_1,c0,c1,c2,c3);
  gpnp4::compute(groebnerMatrix4);
  factors[0] = -groebnerMatrix4(23,36)/groebnerMatrix4(23,35);
  factors[1] =
      -(groebnerMatrix4(22,35)*factors[0]+groebnerMatrix4(22,36))/
      groebnerMatrix4(22,34);
  factors[2] =
      -(groebnerMatrix4(21,34)*factors[1]+groebnerMatrix4(21,35)*factors[0]+
      groebnerMatrix4(21,36))/groebnerMatrix4(21,33);
  factors.push_back(
      -(groebnerMatrix4(20,33)*factors[2]+groebnerMatrix4(20,34)*factors[1]+
      groebnerMatrix4(20,35)*factors[0]+groebnerMatrix4(20,36))/
      groebnerMatrix4(20,32));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using five nullspace vectors
  Eigen::Matrix<double,44,80> groebnerMatrix5 =
      Eigen::Matrix<double,44,80>::Zero();
  gpnp5::init(groebnerMatrix5,a,vec_5,vec_4,vec_3,vec_2,vec_1,c0,c1,c2,c3);
  gpnp5::compute(groebnerMatrix5);
  factors[0] = -groebnerMatrix5(42,79)/groebnerMatrix5(42,78);
  factors[1] =
      -(groebnerMatrix5(41,78)*factors[0]+groebnerMatrix5(41,79))/
      groebnerMatrix5(41,77);
  factors[2] =
      -(groebnerMatrix5(40,77)*factors[1]+groebnerMatrix5(40,78)*factors[0]+
      groebnerMatrix5(40,79))/groebnerMatrix5(40,76);
  factors[3] =
      -(groebnerMatrix5(39,76)*factors[2]+groebnerMatrix5(39,77)*factors[1]+
      groebnerMatrix5(39,78)*factors[0]+groebnerMatrix5(39,79))/
      groebnerMatrix5(39,75);
  factors.push_back(
      -(groebnerMatrix5(38,75)*factors[3]+groebnerMatrix5(38,76)*factors[1]+
      groebnerMatrix5(38,77)*factors[1]+groebnerMatrix5(38,78)*factors[0]+
      groebnerMatrix5(38,79))/groebnerMatrix5(38,74));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //find best solution
  double smallestError = errors.at(0);
  int minimumIndex = 0;
  for( int i = 1; i < 6; i++ )
  {
    if( errors.at(i) < smallestError )
    {
      smallestError = errors.at(i);
      minimumIndex = i;
    }
  }

  transformation.col(3) = ts.at(minimumIndex);
  transformation.block<3,3>(0,0) = Rs.at(minimumIndex);
}

double
opengv::absolute_pose::modules::gpnp_evaluate(
    const Eigen::Matrix<double,12,1> & solution,
    const points_t & c,
    translation_t & t,
    rotation_t & R )
{
  points_t ccam;
  for(size_t i = 0; i<4; i++)
    ccam.push_back(solution.block<3,1>(i*3,0));

  transformation_t transformation = math::arun_complete(c,ccam);
  t = transformation.col(3);
  R = transformation.block<3,3>(0,0);

  //transform world points into camera frame and compute the error
  double error = 0.0;
  for(size_t i = 0; i<4; i++)
  {
    point_t ccam_reprojected = R.transpose() * (c[i] - t);
    error +=
        1.0 -
        (ccam_reprojected.dot(ccam[i])/(ccam[i].norm()*ccam_reprojected.norm()));
  }

  return error;
}

namespace opengv
{
namespace absolute_pose
{
namespace modules
{

struct GpnpOptimizationFunctor : OptimizationFunctor<double>
{
  const Eigen::Matrix<double,12,1> & _a;
  const Eigen::Matrix<double,12,12> & _V;
  const points_t & _c;
  size_t _dim;

  GpnpOptimizationFunctor(
      const Eigen::Matrix<double,12,1> & a,
      const Eigen::Matrix<double,12,12> & V,
      const points_t & c,
      size_t dim ) :
      OptimizationFunctor<double>(dim,6),
      _a(a),
      _V(V),
      _c(c),
      _dim(dim) {}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == _dim );
    assert( (unsigned int) fvec.size() == 6);

    Eigen::Matrix<double,12,1> solution = _a;
    for(size_t i = 0; i < _dim; i++)
      solution += x[i]*_V.col(12-_dim+i);

    points_t ccam;
    for(size_t i = 0; i<4; i++)
      ccam.push_back(solution.block<3,1>(i*3,0));

    Eigen::Vector3d diffw;
    Eigen::Vector3d diffc;
    size_t index = 0;

    for(size_t i = 0; i<3; i++)
    {
      for(size_t j = i+1; j < 4; j++)
      {
        diffw = _c[i]-_c[j];
        diffc = ccam[i]-ccam[j];
        fvec[index++] = diffw.dot(diffw)-diffc.dot(diffc);
      }
    }

    return 0;
  }
};

}
}
}

void
opengv::absolute_pose::modules::gpnp_optimize(
    const Eigen::Matrix<double,12,1> & a,
    const Eigen::Matrix<double,12,12> & V,
    const points_t & c,
    std::vector<double> & factors )
{
  const int n=factors.size();
  VectorXd x(n);

  for(size_t i = 0; i < factors.size(); i++)
    x[i] = factors[i];

  GpnpOptimizationFunctor functor( a, V, c, factors.size() );
  NumericalDiff<GpnpOptimizationFunctor> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<GpnpOptimizationFunctor> > lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 1.E10*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E10*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 1000;
  lm.minimize(x);

  for(size_t i = 0; i < factors.size(); i++)
    factors[i] = x[i];
}

void
opengv::absolute_pose::modules::upnp_fill_s(
    const Eigen::Vector4d & quaternion,
    Eigen::Matrix<double,10,1> & s )
{
  s[0] = quaternion[0] * quaternion[0];
  s[1] = quaternion[1] * quaternion[1];
  s[2] = quaternion[2] * quaternion[2];
  s[3] = quaternion[3] * quaternion[3];
  s[4] = quaternion[0] * quaternion[1];
  s[5] = quaternion[0] * quaternion[2];
  s[6] = quaternion[0] * quaternion[3];
  s[7] = quaternion[1] * quaternion[2];
  s[8] = quaternion[1] * quaternion[3];
  s[9] = quaternion[2] * quaternion[3];
}

//we use this one if the number of correspondences is pretty low (more robust)
void
opengv::absolute_pose::modules::upnp_main(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > & quaternions )
{
  Eigen::Matrix<double,16,16> Action;
  upnp::setupAction_gj( M, C, gamma, Action );
  Eigen::ComplexEigenSolver< Eigen::Matrix<double,16,16> > Eig( Action, true );
  Eigen::Matrix<std::complex<double>,16,16> V = Eig.eigenvectors();

  //cut the double solutions
  double doubleSolThreshold = 0.00000001;

  for( int i = 0; i < 16; i++ )
  {
    //we decided to drop the test for imaginary part
    //I've noticed that when the number of points is really low, things get a little
    //weary with noise, and complex solutions might actually be pretty good

    Eigen::Vector4d quaternion;
    double norm = 0.0;
    for( int q = 0; q < 4; q++ )
    {
      quaternion[q] = V(11+q,i).real();
      norm += pow(quaternion[q],2.0);
    }
    norm = sqrt(norm);
    if(quaternion[0] < 0) // this here is maybe risky, what if quaternion[0] is very small
      norm *= -1.0;
    for( int q = 0; q < 4; q++ )
      quaternion[q] /= norm;

    bool alreadyThere = false;
    for( size_t s = 0; s < quaternions.size(); s++ )
    {
      Eigen::Vector4d diff = quaternion - quaternions[s].second;
      if( diff.norm() < doubleSolThreshold )
      {
        alreadyThere = true;
        break;
      }
    }

    if( !alreadyThere )
    {
      Eigen::Matrix<double,10,1> s;
      upnp_fill_s(quaternion,s);
      Eigen::Matrix<double,1,1> valueM = s.transpose() * M * s + 2.0 * C * s;
      double value = valueM[0] + gamma;

      std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > >::iterator
          qidx = quaternions.begin();
      while( qidx != quaternions.end() && qidx->first < value )
        qidx++;

      quaternions.insert(qidx,std::pair<double,Eigen::Vector4d>(value,quaternion));
    }
  }
}

//this one is the really fast, symmetric version, that we use in the normal case
void
opengv::absolute_pose::modules::upnp_main_sym(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > & quaternions )
{
  Eigen::Matrix<double,8,8> Action;
  upnp::setupAction_sym_gj( M, C, gamma, Action );
  Eigen::ComplexEigenSolver< Eigen::Matrix<double,8,8> > Eig( Action, true );
  Eigen::Matrix<std::complex<double>,8,8> V = Eig.eigenvectors();

  //ok, let's cut the imaginary solutions (with a reasonable threshold!)
  double imagThreshold = 0.01;
  std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > bad_quaternions;

  Eigen::Matrix<std::complex<double>,8,1> D = Eig.eigenvalues();
  for( int i = 0; i < 8; i++ )
  {
    Eigen::Vector4d quaternion;
    quaternion[3] = V(7,i).real();
    quaternion[2] = V(6,i).real();
    quaternion[1] = V(5,i).real();
    quaternion[0] = V(4,i).real();

    double norm = 0.0;
    for( int q = 0; q < 4; q++ )
      norm += pow(quaternion[q],2.0);
    norm = sqrt(norm);
    for( int q = 0; q < 4; q++ )
      quaternion[q] /= norm;

    Eigen::Matrix<double,10,1> s;
    upnp_fill_s(quaternion,s);
    Eigen::Matrix<double,1,1> valueM = s.transpose() * M * s + 2.0 * C * s;
    double value = valueM[0] + gamma;

    if( true )//fabs(D[i].imag()) < imagThreshold ) //use all results for the moment
    {
      std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > >::iterator
          qidx = quaternions.begin();
      while( qidx != quaternions.end() && qidx->first < value )
        qidx++;

      quaternions.insert(qidx,std::pair<double,Eigen::Vector4d>(value,quaternion));
    }
    else
    {
      std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > >::iterator
          qidx = bad_quaternions.begin();
      while( qidx != bad_quaternions.end() && qidx->first < value )
        qidx++;

      bad_quaternions.insert(qidx,std::pair<double,Eigen::Vector4d>(value,quaternion));
    }
  }
  if( quaternions.size() == 0 )
    quaternions = bad_quaternions;
}


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

#include <boost/simd/toolbox/arithmetic/functions/divceil.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/ceil.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/divs.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/constants/one.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                              ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return ceil(tofloat(a0)/tofloat(a1)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<unsigned_<A0>,X>))
                              ((simd_<unsigned_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return rdivide(a0+a1-One<A0>(), a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<int16_<A0>,X>))
                              ((simd_<int16_<A0>,X>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type          itype;
      typedef simd::native<itype,X>                       ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::fusion::tie(a0l, a0h) = split(a0);
      boost::fusion::tie(a1l, a1h) = split(a1);
      return bitwise_cast<A0>(group(divceil(a0l, a1l),divceil(a0h, a1h)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<int8_<A0>,X>))
                              ((simd_<int8_<A0>,X>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type          itype;
      typedef simd::native<itype, X>                      ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::fusion::tie(a0l, a0h) = split(a0);
      boost::fusion::tie(a1l, a1h) = split(a1);
      return simd::bitwise_cast<A0>(group(divceil(a0l, a1l),divceil(a0h, a1h) ));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return ceil(a0/a1); }
  };
} } }

#endif


/*--
  Open3DMotion
  Copyright (c) 2004-2013.
  All rights reserved.
  See LICENSE.txt for more information.
--*/

#include "Open3DMotion/Biomechanics/Algorithms/MOSHFIT/RigidBodyShape.h"

#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
extern "C"
{
#include <f2clibs/f2c.h>
#include <clapack.h>
}
#else
#include <Eigen/Dense>
#include <Eigen/SVD>
#endif

namespace Open3DMotion
{
  const UInt32 RigidBodyResult::success = 0;
  const UInt32 RigidBodyResult::timesequence_mismatch = 1;
  const UInt32 RigidBodyResult::visibility_disconnected = 2;
  const UInt32 RigidBodyResult::insufficient_points = 3;
  const UInt32 RigidBodyResult::did_not_converge = 4;

	RigidBodyShape::RigidBodyShape()
	{
	}

	RigidBodyShape::RigidBodyShape(const RigidBodyShape& src)
	{
		*this = src;
	}

	const RigidBodyShape& RigidBodyShape::operator=(const RigidBodyShape& src)
	{
		marker.assign( src.marker.begin(), src.marker.end() );
		return *this;
	}

	RigidBodyShape::~RigidBodyShape()
	{
	}

	void RigidBodyShape::AddMarker(const double* position, bool visible)
	{
		marker.resize(NumMarkers()+1);
		marker.back().position = position;
		marker.back().visible = visible ? 1 : 0;
	}

	bool RigidBodyShape::IsVisibilitySupersetOf(const RigidBodyShape& other) const
	{
		if (marker.size() == other.marker.size())
		{
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_other->visible && !iter_this->visible)
					return false;
			}
			return true;
		}
		else
		{
			return false;
		}
	}

	bool RigidBodyShape::IsVisibilitySubsetOf(const RigidBodyShape& other) const
	{
		if (marker.size() == other.marker.size())
		{
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_this->visible && !iter_other->visible)
					return false;
			}
			return true;
		}
		else
		{
			return false;
		}
	}

	size_t RigidBodyShape::NumberOfVisibleMarkersInCommonWith(const RigidBodyShape& other) const
	{
		size_t count = 0;
		if (marker.size() == other.marker.size())
		{
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_this->visible && iter_other->visible)
					++count;
			}
		}
		return count;
	}

	void RigidBodyShape::EvaluateNonsingularity3D(std::vector<double>& s, std::vector<double>& coords)
	{
		long num_points(coords.size() / 3);
    s.resize(3);

#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
    long three(3);
		long lwork(256);
		double work[256];
		long info(0);

		std::vector<double> U(9);
		std::vector<double> VT(num_points*num_points);

		// use lapack routine
		// note coords must be column-major so first 3 elements correspond to first coord
		dgesvd_(
			"N",  // don't actually need U
			"N",  // don't actually need VT
			&three, // rows
			&num_points,      // cols
			&coords[0],   // input/output matrix
			&three, // leading dimension of Acpy
			&s[0],      // singular values
			&U[0],      // left orthonormal matrix
			&three, // leading dimension of left
			&VT[0],      // right orthonormal matrix
			&num_points, // leading dimension of right
			work,   // workspace
			&lwork, // size of workspace
			&info);   // returned error codes
#else
		Eigen::Map< Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> >
      _coords(&coords[0], (int)num_points, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 1> > _s(&s[0], 3, 1);
    Eigen::JacobiSVD< Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> > svd(_coords);
    _s = svd.singularValues();
#endif // OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
	}

	bool RigidBodyShape::HasUniqueFitWith(const RigidBodyShape& other, double tolerance) const
	{
		if (marker.size() == other.marker.size())
		{
			size_t num_in_common(0);
			Vector3 common_centroid(0.0);
			std::vector<double> common_coords;
			common_coords.reserve(3*NumMarkers());
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_this->visible && iter_other->visible)
				{
					const Vector3& x = iter_this->position;
					for (size_t j = 0; j < 3; j++)
						common_coords.push_back(x[j]);
					num_in_common++;
					common_centroid += x;
				}
			}

			if (num_in_common >= 3)
			{
				// subract centroid
				common_centroid /= num_in_common;
				double* x = &common_coords[0];
				for (size_t j = 0; j < num_in_common; j++, x+=3)
				{
					Vector3::Sub(x, x, common_centroid);
				}

				// find singular values to estimate how non-colinear these points are
				std::vector<double> s;
				RigidBodyShape::EvaluateNonsingularity3D(s, common_coords);

				// Simplified expression to test points sufficiently non-colinear
				// (condition is sufficient but not always necessary)
				double min_colinearity_allowed = 3.56*sqrt((double)num_in_common)*tolerance;
				if (s[1] > min_colinearity_allowed)
					return true;
			}
		}

		return false;
	}

	UInt32 RigidBodyShape::ComputeFitTo(RigidTransform3& T, const RigidBodyShape& base) const
	{
		size_t num_points = NumMarkers();
		if (num_points == base.NumMarkers())
		{
			// get centroids
			Vector3 centroidA(0,0,0);
			Vector3 centroidB(0,0,0);
			const RigidBodyMarker* iter_A = &base.marker[0];
			const RigidBodyMarker* iter_B = &marker[0];
			size_t ipoint(0);
			size_t num_in_common(0);
			for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++,iter_B++)
			{
				if (iter_A->visible && iter_B->visible)
				{
					Vector3::Add(centroidA.x, centroidA.x, iter_A->position);
					Vector3::Add(centroidB.x, centroidB.x, iter_B->position);
					++num_in_common;
				}
			}

			if (num_in_common >= 3)
			{
				// normalise
				centroidA /= num_in_common;
				centroidB /= num_in_common;

				// correlation matrix
				double sum_correl[9] =
				{ 0.0, 0.0, 0.0,
					0.0, 0.0, 0.0,
					0.0, 0.0, 0.0
				};

				// find correlation matrix
				iter_A = &base.marker[0];
				iter_B = &marker[0];
				for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++, iter_B++)
				{
					if (iter_A->visible && iter_B->visible)
					{
						// input minus centroid
						Vector3 normA;
						Vector3::Sub(normA, iter_A->position, centroidA);

						// cal minus centroid
						Vector3 normB;
						Vector3::Sub(normB, iter_B->position, centroidB);

						// correlation (outer product)
						for (int i = 0; i < 3; i++)
							for (int j = 0; j < 3; j++)
							sum_correl[3*i+j] += normA[i]*normB[j];
					}
				}

				// SVD of correlation to get rotation
				// from mean coords to this frame
				double s[3];
				double U[9], VT[9];
				Matrix3x3::SVD(U, s, VT, sum_correl);

				// force right-handed coord system
				double detU = Matrix3x3::Det(U);
				double detV = Matrix3x3::Det(VT);
				if (detU*detV < 0.0)
				{
					VT[6] *= -1.0;
					VT[7] *= -1.0;
					VT[8] *= -1.0;
				}

				// this is the correct way round
				Matrix3x3::Mul(T.R, U, VT);

				// do post-subtraction of centroid B to get translation vector
				double RcentroidB[3];
				Matrix3x3::MulVec(RcentroidB, T.R, centroidB);
				Vector3::Sub(T.t, centroidA, RcentroidB);

		#if 0
				iter_A = ptA;
				iter_B = ptB;
				for (ipoint = 0; ipoint < npoints; ipoint++, iter_A+=3, iter_B+=3)
				{
					Vector3 Tb;
					RigidTransform3::MulVec(Tb, R, t, iter_B);
					cerr << "x: " << iter_A[0] << " " << Tb[0] << endl;
					cerr << "y: " << iter_A[1] << " " << Tb[1] << endl;
					cerr << "z: " << iter_A[2] << " " << Tb[2] << endl;
					Vector3::Sub(Tb, Tb, iter_A);
					cerr << "Mod: " << Tb.Modulus() << endl;
				}
		#endif

				return RigidBodyResult::success;
			}
		}

		return RigidBodyResult::insufficient_points;
	}

}