text:
/* Copyright (c) 2015 Oleg Morozenkov
 *
 * 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 <boost/test/unit_test.hpp>

#include <test/math/MathFunctions.h>

BOOST_AUTO_TEST_SUITE(tFunctions)

BOOST_AUTO_TEST_CASE(tmin) {
	BOOST_CHECK_EQUAL(min(1, 2), 1);
}

BOOST_AUTO_TEST_CASE(tmax) {
	BOOST_CHECK_EQUAL(max(1, 2), 2);
}

BOOST_AUTO_TEST_CASE(tclamp) {
	BOOST_CHECK_EQUAL(clamp(-0.5, 0, 1), 0);
	BOOST_CHECK_EQUAL(clamp(0.5, 0, 1), 0.5);
	BOOST_CHECK_EQUAL(clamp(1.5, 0, 1), 1);
}

BOOST_AUTO_TEST_CASE(tsign2) {
	BOOST_CHECK_EQUAL(sign2(1, 2), -1);
	BOOST_CHECK_EQUAL(sign2(2, 2), 0);
	BOOST_CHECK_EQUAL(sign2(3, 2), 1);
}

BOOST_AUTO_TEST_CASE(tsign) {
	BOOST_CHECK_EQUAL(sign(-4), -1);
	BOOST_CHECK_EQUAL(sign(0), 0);
	BOOST_CHECK_EQUAL(sign(4), 1);
}

BOOST_AUTO_TEST_SUITE_END()


text:
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <vector>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <limits>
#include <fstream>
#include <set>
#include <utility>
#include <regex>
#include <iomanip>
using namespace std;

unsigned int WIDTH_PX;
 unsigned int HEIGHT_PX;
 float RESOLUTION;
 float OFFSET_X;
 float OFFSET_Y;

  unsigned int ****prev_paths;
  unsigned int ***path_sizes;
  unsigned int ***visited;
  vector< vector< unsigned int > > map_graph;
  vector<vector< unsigned int> > vec;
  vector< unsigned int> temp;

struct vertex{
    unsigned int id, num_neigh;
    float x,y;
    unsigned int id_neigh[8], cost[8];
    double cost_m[8];
    bool visited[8];
    char dir[8][3];
};

struct st_location
{
  unsigned int vertex;
  unsigned int time;
  unsigned int waypoint;

  st_location(unsigned int vertex = 0, unsigned int time = 0, unsigned int waypoint = 0)
    : vertex(vertex), time(time), waypoint(waypoint)
  {
  }
  st_location(const st_location &ref) : vertex(ref.vertex), time(ref.time), waypoint(ref.waypoint)
  {
  }
  bool operator<(const st_location &loc) const
  {
    if (time < loc.time)
    {
      return true;
    }
    else if (time == loc.time)
    {
      if (waypoint > loc.waypoint)
      {
        return true;
      }
    }
    return false;
  }
};

st_location *queue;
vertex *vertex_web;

int FIRST_VID = 7;
unsigned int TEAM_SIZE;
string modalita;
unsigned int dimension;
const unsigned int WHITE = 0, GRAY = 1, BLACK = 2, MAX_TIME = 300U, MAX_WAYPOINTS = 64U;
vector < unsigned int > waypoints;

ofstream myfile;
int guard=0;
set<pair<int,int>> edge_list;
set<int> vertex_list;
pair <int,int> P;
decltype(edge_list)::iterator it;
decltype(edge_list)::iterator itr;
vector <int> tmp_casa ;

template <class T>
vector<unsigned int> spacetime_dijkstra(//const vector< Path > &other_paths,
                                                          const vector <vector <unsigned int> > &vec,
                                                          const vector<vector<unsigned int> > &graph,
                                                          const vector<unsigned int> &waypoints,int start_time,
                                                          vector<unsigned int> *last_leg,
                                                          vector<unsigned int> *first_leg,
                                                          T *cmp_function,int ID_ROBOT)

{

  /*for(int l = 0; l<TEAM_SIZE;l++){
        for(int k = 0;k<vec[l].size();k++){
        cout << "  "<< vec[l][k];
        }cout << endl;
      }*/

  /*cout << "SPACETIME DIJKSTRA --- WAYPOINTS:";
  for (int i = 0; i < waypoints.size(); i++)
  {
    cout << " " << waypoints[i];
  }
  */
  //cout <<endl ;

  unsigned int source = waypoints.front();

  auto it_waypoints = waypoints.begin() +1;
  // inizializzazione strutture

  for (unsigned int i = 0; i < graph.size(); i++)
  {
    for (unsigned int j = 0; j < MAX_TIME; j++)
    {
      for (unsigned int k = 0; k < MAX_WAYPOINTS; k++)
      {
        path_sizes[i][j][k] = 0;
      }
    }
  }
  prev_paths[source][0][1][0] = source;
  path_sizes[source][0][1] = 1;

  for (unsigned int i = 0; i < graph.size(); i++)
  {
    for (unsigned int j = 0; j < MAX_TIME; j++)
    {
      for (unsigned int k = 0; k < MAX_WAYPOINTS; k++)
      {
        visited[i][j][k] = WHITE;
      }
    }
  }

  // inizializzazione coda
  for (unsigned int i = 0; i < MAX_TIME; i++)
  {
    queue[i] = st_location();
  }
  st_location st(source, start_time, 1);
  queue[0] = st;
  int queue_size = 1;

  // individuo la lunghezza del percorso maggiore
  unsigned int max_path_size = 0;
  for (int i = 0; i < TEAM_SIZE; i++)
  {
      //cout <<" size: " << vec[i].size() << endl;
    if (i != ID_ROBOT && vec[i].size() > max_path_size) //
    {
      max_path_size = vec[i].size();
    }
  }

  while (queue_size > 0)
  {
    st_location current_st = queue[--queue_size];
    unsigned int u = current_st.vertex;
    unsigned int time = current_st.time - start_time;
    unsigned int current_waypoint = current_st.waypoint;
    unsigned int next_next_waypoint;

    visited[u][time][current_waypoint] = BLACK;

    if (u == waypoints[current_waypoint])
    {
      if (current_waypoint + 1 == waypoints.size())
      {
        bool good = true;
        for (int i=0; i<TEAM_SIZE && good; i++)
        {
          if (i != ID_ROBOT && vec[i].size() > current_st.time + 1)
          {
            for (int j=current_st.time; j<vec[i].size(); j++)
            {
              if (vec[i][j] == u)
              {
                good = false;
                break;
              }
            }
          }
        }

        //if (time + 1 + start_time >= max_path_size)
        if (good)
        {
          vector<unsigned int> result =
              vector<unsigned int>(prev_paths[u][time][current_waypoint],
                                        prev_paths[u][time][current_waypoint] + path_sizes[u][time][current_waypoint]);
          ///*
          // spezzo il percorso in due parti
          if (last_leg != nullptr && first_leg != nullptr)
          {
            last_leg->clear();
            first_leg->clear();
            bool last_part = true;
            for (auto it = result.rbegin(); it != result.rend(); it++)
            {
              if (*it == waypoints[current_waypoint - 1])
              {
                last_part = false;
              }

              if (last_part)
              {
                last_leg->insert(last_leg->begin(), *it);
              }
              else
              {
                first_leg->insert(first_leg->begin(), *it);
              }
            }
          }
          //*/
          return result;
        }
        else
        {
          next_next_waypoint = current_waypoint;
        }
      }
      else
      {
        next_next_waypoint = current_waypoint + 1;
      }
    }
    else  // u non è waypoint
    {
      next_next_waypoint = current_waypoint;
    }

    // considero l'attesa sul posto
    unsigned int next_time = time + 1;
    if (next_time < MAX_TIME && visited[u][next_time][next_next_waypoint] == WHITE)
    {
      bool good = true;
      for (int i = 0; i < TEAM_SIZE; i++)
      {
        if (i != ID_ROBOT)
        {
          if (next_time + start_time < vec[i].size())
          {
            if (vec[i][next_time + start_time] == u)
            {
              // cout << "can't stand in " << u << " at time " << time << endl;
              good = false;
              break;
            }
          }
          else if (!vec[i].empty() /* && still_robots[i] */

          )

          {
            if (vec[i].back() == u)
            {
              // cout << "can't stand in " << u << " at time " << time << endl;
              good = false;
              break;
            }
          }
        }
      }

      if (good)
      {
        st_location next_st(u, next_time + start_time, next_next_waypoint);
        queue_size = insertion_sort(queue, queue_size, next_st, cmp_function);
        //cout << "o" << endl;
        visited[u][next_time][next_next_waypoint] = GRAY;

        unsigned int psize = path_sizes[u][time][current_waypoint];
        for (unsigned int i = 0; i < psize; i++)
        {
          prev_paths[u][next_time][next_next_waypoint][i] = prev_paths[u][time][current_waypoint][i];
        }

        prev_paths[u][next_time][next_next_waypoint][psize] = u;
        path_sizes[u][next_time][next_next_waypoint] = path_sizes[u][time][current_waypoint] + 1;

      }
    }
    //cout <<"test2613"<<endl;
    // considero i vicini
    for (auto it = graph[u].begin(); it != graph[u].end(); it++)
    {
      const unsigned int v = *it;
      const unsigned int next_time = time + 1;

      if (next_time < MAX_TIME && visited[v][next_time][next_next_waypoint] == WHITE)
      {
        bool good = true;
        for (int i = 0; i < TEAM_SIZE; i++)
        {
          if (i != ID_ROBOT)
          {
            if (next_time + start_time < vec[i].size())
            {
              if (vec[i][next_time + start_time] == v)
              {
                // cout << "can't go in " << v << " at time " << time << endl;
                good = false;
                break;
              }
              if (vec[i][next_time + start_time] == u && vec[i][time + start_time] == v)
              {
                // cout << "can't go in " << v << " at time " << time << endl;
                good = false;
                break;
              }
            }
            else if (!vec[i].empty() /* && still_robots[i] */

            )

            {
              if (vec[i].back() == v) //return ultimo elemento del vettore
              {
                // cout << "can't go in " << v << " at time " << time << ", there is a still robot" << endl;
                good = false;
                break;
              }
            }
          }
        }
        if (good)
        {
          st_location next_st(v, next_time + start_time, next_next_waypoint);
          queue_size = insertion_sort(queue, queue_size, next_st, cmp_function);
          visited[v][next_time][next_next_waypoint] = GRAY;
          unsigned int psize = path_sizes[u][time][current_waypoint];
          for (unsigned int i = 0; i < psize; i++)
          {
            prev_paths[v][next_time][next_next_waypoint][i] = prev_paths[u][time][current_waypoint][i];
          }
          prev_paths[v][next_time][next_next_waypoint][psize] = v;
          path_sizes[v][next_time][next_next_waypoint] = path_sizes[u][time][current_waypoint] + 1;

        }
      }
    }
  }
  throw string("Can't find path!!!");
}

template <class T>
unsigned int insertion_sort(st_location *queue, unsigned int size, st_location loc, T *cmp_function)
{

  int i;
  for (i = size - 1; i >= 0; i--)
  {
    if (cmp_function != nullptr)
    {
      bool cmp_result = (*cmp_function)(loc, queue[i]);

      if (cmp_result)
        break;
    }
    else if (loc < queue[i])
    {
      break;
    }
    queue[i + 1] = queue[i];
    //cout << "vertex: " << queue->vertex << endl;
  }
  queue[i + 1] = loc;
  // std::cout << std::endl;
  return size + 1;
}

void allocate_memory()
{
  cout << "allocating memory..." << endl;
  cout << dimension << " " << MAX_TIME<< endl;

  prev_paths = new unsigned int ***[dimension];
  path_sizes = new unsigned int **[dimension];

  for (unsigned int i = 0; i < dimension; i++)
  {
    prev_paths[i] = new unsigned int **[MAX_TIME];
    path_sizes[i] = new unsigned int *[MAX_TIME];
    for (unsigned int j = 0; j < MAX_TIME; j++)
    {
      prev_paths[i][j] = new unsigned int *[MAX_WAYPOINTS];
      path_sizes[i][j] = new unsigned int[MAX_WAYPOINTS];
      for (unsigned int k = 0; k < MAX_WAYPOINTS; k++)
      {
        prev_paths[i][j][k] = new unsigned int[MAX_TIME];
      }
    }
  }

  visited = new unsigned int **[dimension];
  for (unsigned int i = 0; i < dimension; i++)
  {
    visited[i] = new unsigned int *[MAX_TIME];
    for (unsigned int j = 0; j < MAX_TIME; j++)
    {
      visited[i][j] = new unsigned int[MAX_WAYPOINTS];
    }
  }

  queue = new st_location[MAX_TIME * MAX_TIME];
}

bool astar_cmp_function(const vector < vector < unsigned int > > &min_hops_matrix,
                        const vector<unsigned int> &waypoints, const st_location &lhs, const st_location &rhs)
{
  // uso la min_hops_matrix come euristica sulla lunghezza del percorso rimanente
  unsigned int lhs_h = min_hops_matrix[lhs.vertex][waypoints[lhs.waypoint]];
  unsigned int rhs_h = min_hops_matrix[rhs.vertex][waypoints[rhs.waypoint]];
  // TEST
  // cout << "[DEBUG]\tVertex: " << rhs.vertex << "\tTime: " << rhs.time
  //           << "\tWaypoint: " << waypoints[rhs.waypoint]
  //           << "\tf-value: " << rhs.time + rhs_h << endl;
  if (lhs.time + lhs_h < rhs.time + rhs_h)
  {
    return true;
  }
  // else if (lhs.time + lhs_h == rhs.time + rhs_h)
  // {
  //   if (lhs.waypoint > rhs.waypoint)
  //   {
  //     return true;
  //   }
  // }

  return false;
}

vector< vector< unsigned int > > build_graph()
{
  vector< vector< unsigned int > > result = vector< vector< unsigned int > >(dimension);
  for (int i = 0; i < dimension; i++)
  {
    for (int j = 0; j < vertex_web[i].num_neigh; j++)
    {
      result[i].push_back(vertex_web[i].id_neigh[j]);
    }
  }

  return result;
}

unsigned int GetGraphDimension(const char *graph_file)
{

  FILE *file;
  file = fopen(graph_file, "r");
  unsigned int dimension;

  if (file == NULL)
  {
    printf("Can not open filename %s", graph_file);

  }
  else
  {
    printf("Graph File Opened. Reading Dimensions.\n");
    int r;
    r = fscanf(file, "%u", &dimension);

    //Initialize other dimension variables:
    r = fscanf(file, "%u", &WIDTH_PX);
    r = fscanf(file, "%u", &HEIGHT_PX);
    r = fscanf(file, "%f", &RESOLUTION);
    r = fscanf(file, "%f", &OFFSET_X);
    r = fscanf(file, "%f", &OFFSET_Y);
    //WIDTH_M = (float) WIDTH_PX * RESOLUTION;
    //HEIGHT_M = (float) HEIGHT_PX * RESOLUTION;
    fclose(file);

  }

  return dimension;
}

void GetGraphInfo(vertex *vertex_web, unsigned int dimension, const char *graph_file){

  FILE *file;
  file = fopen(graph_file, "r");

  // FILE *debug;
  // debug = fopen("test-map.graph", "w");

    unsigned int i, j;
    float temp;
    int r;

    //Start Reading the File from FIRST_VID On:
    for (i = 0; i < FIRST_VID - 1; i++)
    {
      r = fscanf(file, "%f", &temp);
      // fprintf(debug, "%f\n", temp);
    }
    // fprintf(debug, "\n");

    for (i = 0; i < dimension; i++)
    {

      r = fscanf(file, "%u", &vertex_web[i].id);
      // fprintf(debug, "%u\n", vertex_web[i].id);

      r = fscanf(file, "%f", &vertex_web[i].x);
      // fprintf(debug, "%f\n", vertex_web[i].x);
      vertex_web[i].x *= RESOLUTION; //convert to m
      vertex_web[i].x += OFFSET_X;

      r = fscanf(file, "%f", &vertex_web[i].y);
      // fprintf(debug, "%f\n", vertex_web[i].y);
      vertex_web[i].y *= RESOLUTION; //convert to m
      vertex_web[i].y += OFFSET_Y;

      r = fscanf(file, "%u", &vertex_web[i].num_neigh);
      // fprintf(debug, "%u\n", vertex_web[i].num_neigh);

      //( pose [%f %f 0 0] name \"point%u\" color \"black\") \n", vertex_web[i].x, vertex_web[i].y, vertex_web[i].id);

      for (j = 0; j < vertex_web[i].num_neigh; j++)
      {
        r = fscanf(file, "%u", &vertex_web[i].id_neigh[j]);
        // fprintf(debug, "%u\n", vertex_web[i].id_neigh[j]);
        //myfile <<" -- "+vertex_web[i].id_neigh[j]+" ;\n";
        P.first=vertex_web[i].id;
        P.second=vertex_web[i].id_neigh[j];
        it = edge_list.find(pair<int,int>(P.first,P.second));
        itr = edge_list.find(pair<int,int>(P.second,P.first));

        if (vertex_list.find(P.first)== vertex_list.end() && ( vertex_list.find(P.second)== vertex_list.end())){ // entrambi i nodi NON presenti nel set
            vertex_list.insert(P.first);
            vertex_list.insert(P.second);
            edge_list.insert(make_pair(P.first,P.second));
          //myfile << vertex_web[i].id;
          //myfile << " -- ";
          //myfile << vertex_web[i].id_neigh[j];
          //myfile << ";\n";
        }
        else if(vertex_list.find(P.first)!= vertex_list.end() && vertex_list.find(P.second)== vertex_list.end()) { // il primo nodo c'è già, il secondo no
            vertex_list.insert(P.second);
            edge_list.insert(make_pair(P.first,P.second));
          //myfile << vertex_web[i].id;
          //myfile << " -- ";
          //myfile << vertex_web[i].id_neigh[j];
          //myfile << ";\n";
        }
        else if(vertex_list.find(P.second)!= vertex_list.end() && vertex_list.find(P.first)== vertex_list.end()) { //il primo non c'è, il secondo è già presente
            vertex_list.insert(P.first);
            edge_list.insert(make_pair(P.first,P.second));
          //myfile << vertex_web[i].id;
          //myfile << " -- ";
          //myfile << vertex_web[i].id_neigh[j];
          //myfile << ";\n";
        }
        else if(vertex_list.find(P.second)!= vertex_list.end() && vertex_list.find(P.first)!= vertex_list.end()) { // entrambi i nodi presenti
            if((it == edge_list.end()) && (itr == edge_list.end())){
                edge_list.insert(make_pair(P.first,P.second));
            //myfile << vertex_web[i].id;
            //myfile << " -- ";
            //myfile << vertex_web[i].id_neigh[j];
            //myfile << ";\n";
          }
        }

        r = fscanf(file, "%s", vertex_web[i].dir[j]);
        // fprintf(debug, "%s\n", vertex_web[i].dir[j]);
        r = fscanf(file, "%u", &vertex_web[i].cost[j]); //can eventually be converted to meters also...
        // fprintf(debug, "%u\n", vertex_web[i].cost[j]);
      }

      // fprintf(debug, "\n");
    }

  // printf("[v=10], x = %f (meters)\n", vertex_web[10].x);

  // fclose(debug);
  fclose(file);
}

unsigned int GetNumberEdges(vertex *vertex_web, unsigned int dimension)
{

  unsigned int result = 0;

  for (unsigned int i = 0; i < dimension; i++)
  {
    for (unsigned int j = 0; j < vertex_web[i].num_neigh; j++)
    {
      if (vertex_web[i].id < vertex_web[i].id_neigh[j])
      {
        result++;
      }
    }
  }

  return result;
}

vector < vector < unsigned int > > calculate_min_hops_matrix()
{
  unsigned int infinity = numeric_limits < unsigned int > ::max();
  vector < vector < unsigned int > > result(dimension,
                                                // std::vector<unsigned int>(dimension, 0));
                                                vector<unsigned int>(dimension, infinity));
  for (unsigned int u = 0; u < dimension; u++)
  {
    // self loop is 1 because the robot must wait for the others to do their moves
    result[u][u] = 1;
    for (unsigned int v : map_graph[u])
    {
      result[u][v] = 1;
    }
  }

  for (unsigned int k = 0; k < dimension; k++)
  {
    for (unsigned int i = 0; i < dimension; i++)
    {
      for (unsigned int j = 0; j < dimension; j++)
      {
        if (result[i][k] != infinity && result[k][j] != infinity && result[i][j] > result[i][k] + result[k][j])
        {
          result[i][j] = result[i][k] + result[k][j];
        }
      }
    }
  }

  return result;
}

vector<vector<unsigned int>> GetWaypoints(const char *waypoint_file, string modalita, string ar)
{

  vector<vector<unsigned int> > wayp;
  vector < unsigned int > waypointsR;
    unsigned int i, j;
    int temp,start;
    int r,c;
    FILE *file;
    file = fopen(waypoint_file, "r");
    ifstream File(waypoint_file);
    string s;
    istringstream is(s);
    int d=2;
    string tmp_path;

    for (i = 0; i < TEAM_SIZE; i++){
      std::vector<unsigned int> pathR;
      c=2;
        while(getline(File, s) && !s.empty()){
          r = fscanf(file, "%d",&temp);
          if(c == 3){
              waypointsR.push_back(temp);
              //tmp_path = tmp_path+"\n"+to_string(temp);
          }

          if(c == 2){
              start = temp;
              //myfile << start;
              //myfile << " [label= \"";
              //myfile << start;
              //myfile << ",R";
              //myfile << i;
              //myfile << "\"];\n";
              waypointsR.push_back(start);
              //tmp_path = to_string(start);

              if(modalita == "S"){
                pathR.push_back(start);
                vec[i]=pathR;
              }
              c=3;
          }
        }
        //tmp_path = tmp_path+"\n"+to_string(waypointsR[waypointsR.size()-1]);
        //myfile << waypointsR[waypointsR.size()-1];
        //myfile << " [label= \"";
        //myfile << waypointsR[waypointsR.size()-1];
        //myfile << ",G";
        //myfile << i;
        //myfile << "\"];\n";
        if (ar == "r"){
          switch (start)
          {
            case 0:
            waypointsR.push_back(0);
            break;
          case 5:
            waypointsR.push_back(5);
            break;
          case 7:
            waypointsR.push_back(7);
            break;
          case 21:
            waypointsR.push_back(8);
            break;
          case 16:
            waypointsR.push_back(3);
            break;
          case 29:
            waypointsR.push_back(10);
            break;
          case 32:
            waypointsR.push_back(9);
            break;
          case 12:
            waypointsR.push_back(8);
            break;
          case 25:
            waypointsR.push_back(2);
            break;
          case 1:
            waypointsR.push_back(1);
            break;
          case 3:
            waypointsR.push_back(3);
            break;
          case 13:
            waypointsR.push_back(13);
            break;
          case 9:
            waypointsR.push_back(9);
            break;
          case 8:
            waypointsR.push_back(8);
            break;
          case 11:
            waypointsR.push_back(11);
            break;
          case 2:
            waypointsR.push_back(2);
            break;
          case 10:
            waypointsR.push_back(10);
            break;

          default:
            break;
          }
        } //rientro a casa, nel nodo iniziale

    wayp.push_back(waypointsR);
    //cout << "current path: "<<tmp_path<< endl;
    //mypaths[i]=tmp_path;
    waypointsR.clear();
    }
  fclose(file);
   // riscrivi il fle waypoint con la nuova combinazione
  return wayp;
}

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

  TEAM_SIZE = atoi(argv[2]);
  string modalita =  argv[3];
  string ar = "r";//argv[4];

  ofstream myfile_output;
  myfile_output.open("astar_output2.txt",ios::out);
  float niteration=0;
  fstream myfile_trunc;
  string mypaths[] = {"0\n6","1\n9","7\n3"};
  std::sort (mypaths,mypaths+TEAM_SIZE);
  regex new_line ("\n+");

  if(modalita=="A"){
    for (int k = 0; k<TEAM_SIZE;k++){
      std::vector<unsigned int> init_vector ;
      vec.push_back(init_vector);
    }
  }

  vector<unsigned int> last_leg, first_leg;
  //myfile.open("/home/riccardoastolfi/minisim2/test_graph.gv");
  //myfile << "graph G{\n"; il resto viene scritto dentro getwaypoint

  string mappa = "src/logistic_sim/maps/"+string(argv[1])+"/"+string(argv[1])+".graph";
  string lista_wayp = "src/logistic_sim/maps/"+string(argv[1])+"/waypoints.txt";
  vector<vector<unsigned int> > waypoints;

  /*if(modalita=="S"){
    for(int l = 0; l<TEAM_SIZE;l++){
        cout << "stato iniziale R" << l << ":";
        for(int k = 0;k<vec[l].size();k++){
        cout << " "<< vec[l][k];
        }
        cout << endl;
    }
  }*/
  dimension = GetGraphDimension(mappa.c_str());

  vertex_web = new vertex[dimension];

  GetGraphInfo(vertex_web, dimension,mappa.c_str());
  //myfile << " }";
  //myfile.close();
  unsigned int nedges = GetNumberEdges(vertex_web, dimension);
  cout << "number of edges "<< nedges << endl;
  cout << "number of nodes "<< dimension << endl;
  //cout << ("Loaded graph %s with %d nodes and %d edges\n", "test", dimension, nedges) << endl;
  map_graph = build_graph();

  vector<vector< unsigned int> > min_hops_matrix = calculate_min_hops_matrix();
  allocate_memory();

  vector<unsigned int>astar_result;
  vector<unsigned int> init_vector ;

  int ok=0;
  int errori = 0;
  int max_timesteps=0;
  vector <string>  tmp_max_steps;
  string tmp_max_array;
  int counter_max_array =0;
  int media_robot_path = 0;
  float media_medie_path = 0;
  float each_robot_media[TEAM_SIZE]={0};
  vector < int > tmp_robot_media;
  float sigma =0;
  vector <float>sigma_collection;

  do{

    myfile_trunc.open(lista_wayp,ios::out);
    for(int f=0;f<TEAM_SIZE;f++){
      myfile_trunc << mypaths[f] ;
      myfile_trunc << "\n";
      myfile_trunc << "\n";
    }

    myfile_trunc.close();

    waypoints = GetWaypoints(lista_wayp.c_str(),modalita,ar);

    //if(modalita=="A"){
      /*for(int l = 0; l<TEAM_SIZE;l++){
        cout << "stato iniziale R" << l << ":";
        for(int k = 0;k<vec[l].size();k++){
          cout << " "<< vec[l][k];
        }
        cout << endl;
      }*/
    //}
    tmp_max_array = "";
    //scrivi sul file l'istanza
    for(int i = 0;i<TEAM_SIZE;i++){
      auto result = std::regex_replace(mypaths[i], new_line, "-");
      myfile_output << result << " ";
      tmp_max_array = tmp_max_array +" " + result;
    }
    myfile_output << endl;
    myfile_output << endl;

    for (int j=0;j<TEAM_SIZE;j++){
      try{
        auto f = boost::bind(astar_cmp_function, min_hops_matrix, waypoints[j], _1, _2);
        astar_result = spacetime_dijkstra(vec,map_graph, waypoints[j],0,&last_leg,&first_leg,&f,j);
        myfile_output << "\tpath R"<< j << ":" << "("<<astar_result.size() << ") [";

        for (int i = 0; i < astar_result.size(); i++){
            myfile_output << " "<< astar_result[i];
        }
        myfile_output << " ]" << endl;

        vec[j] = astar_result;

        //statistical data
        if(astar_result.size() >= max_timesteps){
          counter_max_array ++;
          max_timesteps = astar_result.size();
          tmp_max_array + to_string(j);
          tmp_max_steps.push_back(tmp_max_array);
        }

        each_robot_media[j]= each_robot_media[j] + astar_result.size();
        media_robot_path = media_robot_path + astar_result.size();
        sigma_collection.push_back(astar_result.size());

        myfile_output << "\t";
        for(int l = 0; l<=j;l++){
          for(int k = 0;k<vec[l].size();k++){
              myfile_output <<setw(3)<< vec[l][k];
          }
          myfile_output << endl;
          myfile_output << "\t";
        }

        myfile_output << endl;

        //plot grafo
        //system("dot -Tpng test_graph.gv -o test_graph.png");
        //system("display test_graph.png");

      }catch(string errore){
        errori++;
        //scrivi path errore e modalità
        /*for(int i = 0;i<TEAM_SIZE;i++){
          auto result = std::regex_replace(mypaths[i], new_line, "-");
          myfile_output << result << " ";
        }*/

        myfile_output << endl;
        myfile_output << "result: R"<< j << " non può completare";
        myfile_output << endl;
        myfile_output << " MODE: " << modalita << endl;
        break;
      }//catch
    } //fine for (per ogni robot all'interno di un ordine)

    media_robot_path = media_robot_path/TEAM_SIZE;
    myfile_output << "timesteps medio corrente: " << media_robot_path << endl;
    media_medie_path = media_medie_path + media_robot_path;
    media_robot_path = 0;
    myfile_output << "___________________________________________________________________________________________________________" << endl;
    myfile_output << endl;
    myfile_output << endl;
    myfile_output << endl;

    //ri inizializza tutta la struttura dopo ogni combinazione
    for (int k = 0; k<TEAM_SIZE;k++){
      vec[k]=init_vector;
    }

    //cout << "n iteration: "<< niteration << endl;
    /*for(int i=0;i<TEAM_SIZE;i++){
      cout << mypaths[i] << endl;
    }*/
    niteration++;

  } while ( std::next_permutation(mypaths,mypaths+TEAM_SIZE));//fine do
  media_medie_path = media_medie_path/niteration;
  myfile_output.close();
  cout << "errori : "<<errori<<endl;
  cout << "max timesteps: " << max_timesteps << " "<< counter_max_array<< "volte" << endl;
  cout << "ottenuto nelle seguenti configurazioni" << endl;

  /*for(int l = 0; l<tmp_max_steps.size();l++){
    //cout << "\t "<< tmp_max_steps[l].substr(0,(sizeof(tmp_max_steps[l])/sizeof(tmp_max_steps[l][0]))-1)<<endl;
    cout << "\t "<< tmp_max_steps[l]<<endl;
    //cout <<" Robot R" << tmp_max_steps[l].substr((sizeof(tmp_max_steps[l])/sizeof(tmp_max_steps[l][0]))-2,(sizeof(tmp_max_steps[l])/sizeof(tmp_max_steps[l][0]))-1) << endl;
    cout << endl;
  }*/
  cout << "media medie path: " << media_medie_path << endl;

  for(int i=0;i<TEAM_SIZE;i++){
      cout << "robot R"<< i << " path globale medio: " << each_robot_media[i]/niteration << endl; //media dei percorsi fatti da ciascun robot
  }

  //deviazione standard
  for(int i = 0;i<sigma_collection.size();i++){
    sigma = sigma + ( pow((sigma_collection[i]-media_medie_path),2) );
  }
  sigma = sqrt(sigma/niteration);
  cout << "deviazione standard: " << sigma << endl;
}

ext: cpp

max_stars_repo_name: Astuxx/RMAPD

max_issues_repo_name: Astuxx/RMAPD

max_forks_repo_name: Astuxx/RMAPD


text:
// This file is auto-generated, don't edit it. Thanks.

#ifndef ALIBABACLOUD_OPENAPI_H_
#define ALIBABACLOUD_OPENAPI_H_

#include <alibabacloud/credential.hpp>
#include <boost/any.hpp>
#include <boost/throw_exception.hpp>
#include <darabonba/core.hpp>
#include <darabonba/util.hpp>
#include <iostream>
#include <map>

using namespace std;

namespace Alibabacloud_OpenApi {
class Config : public Darabonba::Model {
public:
  shared_ptr<string> accessKeyId{};
  shared_ptr<string> accessKeySecret{};
  shared_ptr<string> securityToken{};
  shared_ptr<string> protocol{};
  shared_ptr<string> regionId{};
  shared_ptr<int> readTimeout{};
  shared_ptr<int> connectTimeout{};
  shared_ptr<string> httpProxy{};
  shared_ptr<string> httpsProxy{};
  shared_ptr<Alibabacloud_Credential::Client> credential{};
  shared_ptr<string> endpoint{};
  shared_ptr<string> noProxy{};
  shared_ptr<int> maxIdleConns{};
  shared_ptr<string> network{};
  shared_ptr<string> userAgent{};
  shared_ptr<string> suffix{};
  shared_ptr<string> socks5Proxy{};
  shared_ptr<string> socks5NetWork{};
  shared_ptr<string> endpointType{};
  shared_ptr<string> openPlatformEndpoint{};
  shared_ptr<string> type{};
  shared_ptr<string> signatureAlgorithm{};

  Config() {}

  explicit Config(const std::map<string, boost::any> &config)
      : Darabonba::Model(config) {
    fromMap(config);
  };

  void validate() override {}

  map<string, boost::any> toMap() override {
    map<string, boost::any> res;
    if (accessKeyId) {
      res["accessKeyId"] = boost::any(*accessKeyId);
    }
    if (accessKeySecret) {
      res["accessKeySecret"] = boost::any(*accessKeySecret);
    }
    if (securityToken) {
      res["securityToken"] = boost::any(*securityToken);
    }
    if (protocol) {
      res["protocol"] = boost::any(*protocol);
    }
    if (regionId) {
      res["regionId"] = boost::any(*regionId);
    }
    if (readTimeout) {
      res["readTimeout"] = boost::any(*readTimeout);
    }
    if (connectTimeout) {
      res["connectTimeout"] = boost::any(*connectTimeout);
    }
    if (httpProxy) {
      res["httpProxy"] = boost::any(*httpProxy);
    }
    if (httpsProxy) {
      res["httpsProxy"] = boost::any(*httpsProxy);
    }
    if (endpoint) {
      res["endpoint"] = boost::any(*endpoint);
    }
    if (noProxy) {
      res["noProxy"] = boost::any(*noProxy);
    }
    if (maxIdleConns) {
      res["maxIdleConns"] = boost::any(*maxIdleConns);
    }
    if (network) {
      res["network"] = boost::any(*network);
    }
    if (userAgent) {
      res["userAgent"] = boost::any(*userAgent);
    }
    if (suffix) {
      res["suffix"] = boost::any(*suffix);
    }
    if (socks5Proxy) {
      res["socks5Proxy"] = boost::any(*socks5Proxy);
    }
    if (socks5NetWork) {
      res["socks5NetWork"] = boost::any(*socks5NetWork);
    }
    if (endpointType) {
      res["endpointType"] = boost::any(*endpointType);
    }
    if (openPlatformEndpoint) {
      res["openPlatformEndpoint"] = boost::any(*openPlatformEndpoint);
    }
    if (type) {
      res["type"] = boost::any(*type);
    }
    if (signatureAlgorithm) {
      res["signatureAlgorithm"] = boost::any(*signatureAlgorithm);
    }
    return res;
  }

  void fromMap(map<string, boost::any> m) override {
    if (m.find("accessKeyId") != m.end() && !m["accessKeyId"].empty()) {
      accessKeyId =
          make_shared<string>(boost::any_cast<string>(m["accessKeyId"]));
    }
    if (m.find("accessKeySecret") != m.end() && !m["accessKeySecret"].empty()) {
      accessKeySecret =
          make_shared<string>(boost::any_cast<string>(m["accessKeySecret"]));
    }
    if (m.find("securityToken") != m.end() && !m["securityToken"].empty()) {
      securityToken =
          make_shared<string>(boost::any_cast<string>(m["securityToken"]));
    }
    if (m.find("protocol") != m.end() && !m["protocol"].empty()) {
      protocol = make_shared<string>(boost::any_cast<string>(m["protocol"]));
    }
    if (m.find("regionId") != m.end() && !m["regionId"].empty()) {
      regionId = make_shared<string>(boost::any_cast<string>(m["regionId"]));
    }
    if (m.find("readTimeout") != m.end() && !m["readTimeout"].empty()) {
      readTimeout = make_shared<int>(boost::any_cast<int>(m["readTimeout"]));
    }
    if (m.find("connectTimeout") != m.end() && !m["connectTimeout"].empty()) {
      connectTimeout =
          make_shared<int>(boost::any_cast<int>(m["connectTimeout"]));
    }
    if (m.find("httpProxy") != m.end() && !m["httpProxy"].empty()) {
      httpProxy = make_shared<string>(boost::any_cast<string>(m["httpProxy"]));
    }
    if (m.find("httpsProxy") != m.end() && !m["httpsProxy"].empty()) {
      httpsProxy =
          make_shared<string>(boost::any_cast<string>(m["httpsProxy"]));
    }
    if (m.find("endpoint") != m.end() && !m["endpoint"].empty()) {
      endpoint = make_shared<string>(boost::any_cast<string>(m["endpoint"]));
    }
    if (m.find("noProxy") != m.end() && !m["noProxy"].empty()) {
      noProxy = make_shared<string>(boost::any_cast<string>(m["noProxy"]));
    }
    if (m.find("maxIdleConns") != m.end() && !m["maxIdleConns"].empty()) {
      maxIdleConns = make_shared<int>(boost::any_cast<int>(m["maxIdleConns"]));
    }
    if (m.find("network") != m.end() && !m["network"].empty()) {
      network = make_shared<string>(boost::any_cast<string>(m["network"]));
    }
    if (m.find("userAgent") != m.end() && !m["userAgent"].empty()) {
      userAgent = make_shared<string>(boost::any_cast<string>(m["userAgent"]));
    }
    if (m.find("suffix") != m.end() && !m["suffix"].empty()) {
      suffix = make_shared<string>(boost::any_cast<string>(m["suffix"]));
    }
    if (m.find("socks5Proxy") != m.end() && !m["socks5Proxy"].empty()) {
      socks5Proxy =
          make_shared<string>(boost::any_cast<string>(m["socks5Proxy"]));
    }
    if (m.find("socks5NetWork") != m.end() && !m["socks5NetWork"].empty()) {
      socks5NetWork =
          make_shared<string>(boost::any_cast<string>(m["socks5NetWork"]));
    }
    if (m.find("endpointType") != m.end() && !m["endpointType"].empty()) {
      endpointType =
          make_shared<string>(boost::any_cast<string>(m["endpointType"]));
    }
    if (m.find("openPlatformEndpoint") != m.end() &&
        !m["openPlatformEndpoint"].empty()) {
      openPlatformEndpoint = make_shared<string>(
          boost::any_cast<string>(m["openPlatformEndpoint"]));
    }
    if (m.find("type") != m.end() && !m["type"].empty()) {
      type = make_shared<string>(boost::any_cast<string>(m["type"]));
    }
    if (m.find("signatureAlgorithm") != m.end() &&
        !m["signatureAlgorithm"].empty()) {
      signatureAlgorithm =
          make_shared<string>(boost::any_cast<string>(m["signatureAlgorithm"]));
    }
  }

  virtual ~Config() = default;
};
class OpenApiRequest : public Darabonba::Model {
public:
  shared_ptr<map<string, string>> headers{};
  shared_ptr<map<string, string>> query{};
  shared_ptr<boost::any> body{};
  shared_ptr<Darabonba::Stream> stream{};

  OpenApiRequest() {}

  explicit OpenApiRequest(const std::map<string, boost::any> &config)
      : Darabonba::Model(config) {
    fromMap(config);
  };

  void validate() override {}

  map<string, boost::any> toMap() override {
    map<string, boost::any> res;
    if (headers) {
      res["headers"] = boost::any(*headers);
    }
    if (query) {
      res["query"] = boost::any(*query);
    }
    if (body) {
      res["body"] = boost::any(*body);
    }
    if (stream) {
      res["stream"] = boost::any(*stream);
    }
    return res;
  }

  void fromMap(map<string, boost::any> m) override {
    if (m.find("headers") != m.end() && !m["headers"].empty()) {
      map<string, string> map1 =
          boost::any_cast<map<string, string>>(m["headers"]);
      map<string, string> toMap1;
      for (auto item : map1) {
        toMap1[item.first] = item.second;
      }
      headers = make_shared<map<string, string>>(toMap1);
    }
    if (m.find("query") != m.end() && !m["query"].empty()) {
      map<string, string> map1 =
          boost::any_cast<map<string, string>>(m["query"]);
      map<string, string> toMap1;
      for (auto item : map1) {
        toMap1[item.first] = item.second;
      }
      query = make_shared<map<string, string>>(toMap1);
    }
    if (m.find("body") != m.end() && !m["body"].empty()) {
      body = make_shared<boost::any>(m["body"]);
    }
    if (m.find("stream") != m.end() && !m["stream"].empty()) {
      stream = make_shared<Darabonba::Stream>(
          boost::any_cast<Darabonba::Stream>(m["stream"]));
    }
  }

  virtual ~OpenApiRequest() = default;
};
class Params : public Darabonba::Model {
public:
  shared_ptr<string> action{};
  shared_ptr<string> version{};
  shared_ptr<string> protocol{};
  shared_ptr<string> pathname{};
  shared_ptr<string> method{};
  shared_ptr<string> authType{};
  shared_ptr<string> bodyType{};
  shared_ptr<string> reqBodyType{};
  shared_ptr<string> style{};

  Params() {}

  explicit Params(const std::map<string, boost::any> &config)
      : Darabonba::Model(config) {
    fromMap(config);
  };

  void validate() override {
    if (!action) {
      BOOST_THROW_EXCEPTION(
          boost::enable_error_info(std::runtime_error("action is required.")));
    }
    if (!version) {
      BOOST_THROW_EXCEPTION(
          boost::enable_error_info(std::runtime_error("version is required.")));
    }
    if (!protocol) {
      BOOST_THROW_EXCEPTION(boost::enable_error_info(
          std::runtime_error("protocol is required.")));
    }
    if (!pathname) {
      BOOST_THROW_EXCEPTION(boost::enable_error_info(
          std::runtime_error("pathname is required.")));
    }
    if (!method) {
      BOOST_THROW_EXCEPTION(
          boost::enable_error_info(std::runtime_error("method is required.")));
    }
    if (!authType) {
      BOOST_THROW_EXCEPTION(boost::enable_error_info(
          std::runtime_error("authType is required.")));
    }
    if (!bodyType) {
      BOOST_THROW_EXCEPTION(boost::enable_error_info(
          std::runtime_error("bodyType is required.")));
    }
    if (!reqBodyType) {
      BOOST_THROW_EXCEPTION(boost::enable_error_info(
          std::runtime_error("reqBodyType is required.")));
    }
  }

  map<string, boost::any> toMap() override {
    map<string, boost::any> res;
    if (action) {
      res["action"] = boost::any(*action);
    }
    if (version) {
      res["version"] = boost::any(*version);
    }
    if (protocol) {
      res["protocol"] = boost::any(*protocol);
    }
    if (pathname) {
      res["pathname"] = boost::any(*pathname);
    }
    if (method) {
      res["method"] = boost::any(*method);
    }
    if (authType) {
      res["authType"] = boost::any(*authType);
    }
    if (bodyType) {
      res["bodyType"] = boost::any(*bodyType);
    }
    if (reqBodyType) {
      res["reqBodyType"] = boost::any(*reqBodyType);
    }
    if (style) {
      res["style"] = boost::any(*style);
    }
    return res;
  }

  void fromMap(map<string, boost::any> m) override {
    if (m.find("action") != m.end() && !m["action"].empty()) {
      action = make_shared<string>(boost::any_cast<string>(m["action"]));
    }
    if (m.find("version") != m.end() && !m["version"].empty()) {
      version = make_shared<string>(boost::any_cast<string>(m["version"]));
    }
    if (m.find("protocol") != m.end() && !m["protocol"].empty()) {
      protocol = make_shared<string>(boost::any_cast<string>(m["protocol"]));
    }
    if (m.find("pathname") != m.end() && !m["pathname"].empty()) {
      pathname = make_shared<string>(boost::any_cast<string>(m["pathname"]));
    }
    if (m.find("method") != m.end() && !m["method"].empty()) {
      method = make_shared<string>(boost::any_cast<string>(m["method"]));
    }
    if (m.find("authType") != m.end() && !m["authType"].empty()) {
      authType = make_shared<string>(boost::any_cast<string>(m["authType"]));
    }
    if (m.find("bodyType") != m.end() && !m["bodyType"].empty()) {
      bodyType = make_shared<string>(boost::any_cast<string>(m["bodyType"]));
    }
    if (m.find("reqBodyType") != m.end() && !m["reqBodyType"].empty()) {
      reqBodyType =
          make_shared<string>(boost::any_cast<string>(m["reqBodyType"]));
    }
    if (m.find("style") != m.end() && !m["style"].empty()) {
      style = make_shared<string>(boost::any_cast<string>(m["style"]));
    }
  }

  virtual ~Params() = default;
};
class Client {
public:
  shared_ptr<string> _endpoint{};
  shared_ptr<string> _regionId{};
  shared_ptr<string> _protocol{};
  shared_ptr<string> _userAgent{};
  shared_ptr<string> _endpointRule{};
  shared_ptr<map<string, string>> _endpointMap{};
  shared_ptr<string> _suffix{};
  shared_ptr<int> _readTimeout{};
  shared_ptr<int> _connectTimeout{};
  shared_ptr<string> _httpProxy{};
  shared_ptr<string> _httpsProxy{};
  shared_ptr<string> _socks5Proxy{};
  shared_ptr<string> _socks5NetWork{};
  shared_ptr<string> _noProxy{};
  shared_ptr<string> _network{};
  shared_ptr<string> _productId{};
  shared_ptr<int> _maxIdleConns{};
  shared_ptr<string> _endpointType{};
  shared_ptr<string> _openPlatformEndpoint{};
  shared_ptr<Alibabacloud_Credential::Client> _credential{};
  shared_ptr<string> _signatureAlgorithm{};
  shared_ptr<map<string, string>> _headers{};
  explicit Client(const shared_ptr<Config> &config);
  map<string, boost::any>
  doRPCRequest(shared_ptr<string> action, shared_ptr<string> version,
               shared_ptr<string> protocol, shared_ptr<string> method,
               shared_ptr<string> authType, shared_ptr<string> bodyType,
               shared_ptr<OpenApiRequest> request,
               shared_ptr<Darabonba_Util::RuntimeOptions> runtime);
  map<string, boost::any>
  doROARequest(shared_ptr<string> action, shared_ptr<string> version,
               shared_ptr<string> protocol, shared_ptr<string> method,
               shared_ptr<string> authType, shared_ptr<string> pathname,
               shared_ptr<string> bodyType, shared_ptr<OpenApiRequest> request,
               shared_ptr<Darabonba_Util::RuntimeOptions> runtime);
  map<string, boost::any>
  doROARequestWithForm(shared_ptr<string> action, shared_ptr<string> version,
                       shared_ptr<string> protocol, shared_ptr<string> method,
                       shared_ptr<string> authType, shared_ptr<string> pathname,
                       shared_ptr<string> bodyType,
                       shared_ptr<OpenApiRequest> request,
                       shared_ptr<Darabonba_Util::RuntimeOptions> runtime);
  map<string, boost::any>
  doRequest(shared_ptr<Params> params, shared_ptr<OpenApiRequest> request,
            shared_ptr<Darabonba_Util::RuntimeOptions> runtime);
  map<string, boost::any>
  callApi(shared_ptr<Params> params, shared_ptr<OpenApiRequest> request,
          shared_ptr<Darabonba_Util::RuntimeOptions> runtime);
  string getUserAgent();
  string getAccessKeyId();
  string getAccessKeySecret();
  string getSecurityToken();
  static boost::any defaultAny(const boost::any &inputValue,
                               const boost::any &defaultValue);
  void checkConfig(shared_ptr<Config> config);
  void setRpcHeaders(shared_ptr<map<string, string>> headers);
  map<string, string> getRpcHeaders();

  virtual ~Client() = default;
};
} // namespace Alibabacloud_OpenApi

#endif

ext: hpp


text:
/**
 * @file LbfgsSolver.hpp
 * @author Jorge Nocedal
 * @license License BSD-3-Clause
 * @copyright 1990, Jorge Nocedal
 * @copyright 2007-2010 Naoaki Okazaki
 * @date 2019-10-06
 * @brief C library of Limited memory BFGS (L-BFGS).
 *
 * All rights reserved.
 *
 * 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.
 *
 */

#pragma once

#include <iostream>
#include <Eigen/Dense>
#include <solver/interface/NlpDescription.hpp>

namespace solver {
  /**
   * Return values of lbfgs().
   *  Roughly speaking, a negative value indicates an error.
   */
  enum {
    /** L-BFGS reaches convergence. */
    LBFGS_SUCCESS = 0,
    LBFGS_STOP,
    /** The initial variables already minimize the objective function. */
    LBFGS_ALREADY_MINIMIZED,
	/** Unknown error. */
	LBFGSERR_UNKNOWNERROR = -1024,
    /** Logic error. */
    LBFGSERR_LOGICERROR,
    /** Insufficient memory. */
    LBFGSERR_OUTOFMEMORY,
    /** Invalid number of variables specified. */
    LBFGSERR_INVALID_N,
    /** Invalid parameter lbfgs_parameter_t::epsilon specified. */
    LBFGSERR_INVALID_EPSILON,
    /** Invalid parameter lbfgs_parameter_t::past specified. */
    LBFGSERR_INVALID_TESTPERIOD,
    /** Invalid parameter lbfgs_parameter_t::delta specified. */
    LBFGSERR_INVALID_DELTA,
    /** Invalid parameter lbfgs_parameter_t::linesearch specified. */
    LBFGSERR_INVALID_LINESEARCH,
    /** Invalid parameter lbfgs_parameter_t::max_step specified. */
    LBFGSERR_INVALID_MINSTEP,
    /** Invalid parameter lbfgs_parameter_t::max_step specified. */
    LBFGSERR_INVALID_MAXSTEP,
    /** Invalid parameter lbfgs_parameter_t::ftol specified. */
    LBFGSERR_INVALID_FTOL,
    /** Invalid parameter lbfgs_parameter_t::wolfe specified. */
    LBFGSERR_INVALID_WOLFE,
    /** Invalid parameter lbfgs_parameter_t::gtol specified. */
    LBFGSERR_INVALID_GTOL,
    /** Invalid parameter lbfgs_parameter_t::xtol specified. */
    LBFGSERR_INVALID_XTOL,
    /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */
    LBFGSERR_INVALID_MAXLINESEARCH,
    /** Invalid parameter lbfgs_parameter_t::orthantwise_c specified. */
    LBFGSERR_INVALID_ORTHANTWISE,
    /** Invalid parameter lbfgs_parameter_t::orthantwise_start specified. */
    LBFGSERR_INVALID_ORTHANTWISE_START,
    /** Invalid parameter lbfgs_parameter_t::orthantwise_end specified. */
    LBFGSERR_INVALID_ORTHANTWISE_END,
    /** The line-search step went out of the interval of uncertainty. */
    LBFGSERR_OUTOFINTERVAL,
    /** A logic error occurred; alternatively, the interval of uncertainty became too small. */
    LBFGSERR_INCORRECT_TMINMAX,
    /** A rounding error occurred; alternatively, no line-search step
        satisfies the sufficient decrease and curvature conditions. */
    LBFGSERR_ROUNDING_ERROR,
    /** The line-search step became smaller than lbfgs_parameter_t::min_step. */
    LBFGSERR_MINIMUMSTEP,
    /** The line-search step became larger than lbfgs_parameter_t::max_step. */
    LBFGSERR_MAXIMUMSTEP,
    /** The line-search routine reaches the maximum number of evaluations. */
    LBFGSERR_MAXIMUMLINESEARCH,
    /** The algorithm routine reaches the maximum number of iterations. */
    LBFGSERR_MAXIMUMITERATION,
    /** Relative width of the interval of uncertainty is at most lbfgs_parameter_t::xtol. */
    LBFGSERR_WIDTHTOOSMALL,
    /** A logic error (negative line-search step) occurred. */
    LBFGSERR_INVALIDPARAMETERS,
    /** The current search direction increases the objective function value. */
    LBFGSERR_INCREASEGRADIENT,
  };

  /**
   * Line search algorithms.
   */
  enum {
    /** The default algorithm (MoreThuente method). */
    LBFGS_LINESEARCH_DEFAULT = 0,
    /** MoreThuente method proposd by More and Thuente. */
    LBFGS_LINESEARCH_MORETHUENTE = 0,
    /**
     * Backtracking method with the Armijo condition.
     *  The backtracking method finds the step length such that it satisfies
     *  the sufficient decrease (Armijo) condition,
     *    - f(x + a * d) <= f(x) + lbfgs_parameter_t::ftol * a * g(x)^T d,
     *
     *  where x is the current point, d is the current search direction, and
     *  a is the step length.
     */
    LBFGS_LINESEARCH_BACKTRACKING_ARMIJO = 1,
    /** The backtracking method with the defualt (regular Wolfe) condition. */
    LBFGS_LINESEARCH_BACKTRACKING = 2,
    /**
     * Backtracking method with regular Wolfe condition.
     *  The backtracking method finds the step length such that it satisfies
     *  both the Armijo condition (LBFGS_LINESEARCH_BACKTRACKING_ARMIJO)
     *  and the curvature condition,
     *    - g(x + a * d)^T d >= lbfgs_parameter_t::wolfe * g(x)^T d,
     *
     *  where x is the current point, d is the current search direction, and
     *  a is the step length.
     */
    LBFGS_LINESEARCH_BACKTRACKING_WOLFE = 2,
    /**
     * Backtracking method with strong Wolfe condition.
     *  The backtracking method finds the step length such that it satisfies
     *  both the Armijo condition (LBFGS_LINESEARCH_BACKTRACKING_ARMIJO)
     *  and the following condition,
     *    - |g(x + a * d)^T d| <= lbfgs_parameter_t::wolfe * |g(x)^T d|,
     *
     *  where x is the current point, d is the current search direction, and
     *  a is the step length.
     */
    LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE = 3,
  };

  /**
   * L-BFGS optimization parameters.
   *  Call lbfgs_parameter_init() function to initialize parameters to the
   *  default values.
   */
  typedef struct {
    /**
     * The number of corrections to approximate the inverse hessian matrix.
     *  The L-BFGS routine stores the computation results of previous \ref m
     *  iterations to approximate the inverse hessian matrix of the current
     *  iteration. This parameter controls the size of the limited memories
     *  (corrections). The default value is \c 6. Values less than \c 3 are
     *  not recommended. Large values will result in excessive computing time.
     */
    int m;

    /**
     * Epsilon for convergence test.
     *  This parameter determines the accuracy with which the solution is to
     *  be found. A minimization terminates when
     *      ||g|| < \ref epsilon * max(1, ||x||),
     *  where ||.|| denotes the Euclidean (L2) norm. The default value is
     *  \c 1e-5.
     */
    double epsilon;

    /**
     * Distance for delta-based convergence test.
     *  This parameter determines the distance, in iterations, to compute
     *  the rate of decrease of the objective function. If the value of this
     *  parameter is zero, the library does not perform the delta-based
     *  convergence test. The default value is \c 0.
     */
    int past;

    /**
     * Delta for convergence test.
     *  This parameter determines the minimum rate of decrease of the
     *  objective function. The library stops iterations when the
     *  following condition is met:
     *      (f' - f) / f < \ref delta,
     *  where f' is the objective value of \ref past iterations ago, and f is
     *  the objective value of the current iteration.
     *  The default value is \c 0.
     */
    double delta;

    /**
     * The maximum number of iterations.
     *  The lbfgs() function terminates an optimization process with
     *  ::LBFGSERR_MAXIMUMITERATION status code when the iteration count
     *  exceedes this parameter. Setting this parameter to zero continues an
     *  optimization process until a convergence or error. The default value
     *  is \c 0.
     */
    int max_iterations;

    /**
     * The line search algorithm.
     *  This parameter specifies a line search algorithm to be used by the
     *  L-BFGS routine.
     */
    int linesearch;

    /**
     * The maximum number of trials for the line search.
     *  This parameter controls the number of function and gradients evaluations
     *  per iteration for the line search routine. The default value is \c 20.
     */
    int max_linesearch;

    /**
     * The minimum step of the line search routine.
     *  The default value is \c 1e-20. This value need not be modified unless
     *  the exponents are too large for the machine being used, or unless the
     *  problem is extremely badly scaled (in which case the exponents should
     *  be increased).
     */
    double min_step;

    /**
     * The maximum step of the line search.
     *  The default value is \c 1e+20. This value need not be modified unless
     *  the exponents are too large for the machine being used, or unless the
     *  problem is extremely badly scaled (in which case the exponents should
     *  be increased).
     */
    double max_step;

    /**
     * A parameter to control the accuracy of the line search routine.
     *  The default value is \c 1e-4. This parameter should be greater
     *  than zero and smaller than \c 0.5.
     */
    double ftol;

    /**
     * A coefficient for the Wolfe condition.
     *  This parameter is valid only when the backtracking line-search
     *  algorithm is used with the Wolfe condition,
     *  ::LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE or
     *  ::LBFGS_LINESEARCH_BACKTRACKING_WOLFE .
     *  The default value is \c 0.9. This parameter should be greater
     *  the \ref ftol parameter and smaller than \c 1.0.
     */
    double wolfe;

    /**
     * A parameter to control the accuracy of the line search routine.
     *  The default value is \c 0.9. If the function and gradient
     *  evaluations are inexpensive with respect to the cost of the
     *  iteration (which is sometimes the case when solving very large
     *  problems) it may be advantageous to set this parameter to a small
     *  value. A typical small value is \c 0.1. This parameter shuold be
     *  greater than the \ref ftol parameter (\c 1e-4) and smaller than
     *  \c 1.0.
     */
    double gtol;

    /**
     * The machine precision for floating-point values.
     *  This parameter must be a positive value set by a client program to
     *  estimate the machine precision. The line search routine will terminate
     *  with the status code (::LBFGSERR_ROUNDING_ERROR) if the relative width
     *  of the interval of uncertainty is less than this parameter.
     */
    double xtol;
  } lbfgs_parameter_t;

  /**
   * Callback interface to provide objective function and gradient evaluations.
   *
   *  The lbfgs() function call this function to obtain the values of objective
   *  function and its gradients when needed. A client program must implement
   *  this function to evaluate the values of the objective function and its
   *  gradients, given current values of variables.
   *
   *  @param  instance    The user data sent for lbfgs() function by the client.
   *  @param  x           The current values of variables.
   *  @param  g           The gradient vector. The callback function must compute
   *                      the gradient values for the current variables.
   *  @param  n           The number of variables.
   *  @param  step        The current step of the line search routine.
   *  @retval double The value of the objective function for the current
   *                          variables.
   */
  typedef double (*lbfgs_evaluate_t)(void *instance, const double *x,
    double *g, const int n, const double step);

  /**
   * Callback interface to receive the progress of the optimization process.
   *
   *  The lbfgs() function call this function for each iteration. Implementing
   *  this function, a client program can store or display the current progress
   *  of the optimization process.
   *
   *  @param  instance    The user data sent for lbfgs() function by the client.
   *  @param  x           The current values of variables.
   *  @param  g           The current gradient values of variables.
   *  @param  fx          The current value of the objective function.
   *  @param  xnorm       The Euclidean norm of the variables.
   *  @param  gnorm       The Euclidean norm of the gradients.
   *  @param  step        The line-search step used for this iteration.
   *  @param  n           The number of variables.
   *  @param  k           The iteration count.
   *  @param  ls          The number of evaluations called for this iteration.
   *  @retval int         Zero to continue the optimization process. Returning a
   *                      non-zero value will cancel the optimization process.
   */
  typedef int (*lbfgs_progress_t)(void *instance, const double *x,
    const double *g, const double fx, const double xnorm,
    const double gnorm, const double step, int n, int k, int ls);

  /*
  A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation
  callback) and pass the pointer to the callback function to lbfgs() arguments.
  Similarly, a user can implement a function compatible with ::lbfgs_progress_t
  (progress callback) to obtain the current progress (e.g., variables, function
  value, ||G||, etc) and to cancel the iteration process if necessary.
  Implementation of a progress callback is optional: a user can pass \c NULL if
  progress notification is not necessary.

  In addition, a user must preserve two requirements:
    - The number of variables must be multiples of 16 (this is not 4).
    - The memory block of variable array ::x must be aligned to 16.

  This algorithm terminates an optimization
  when:

    ||G|| < \epsilon \cdot \max(1, ||x||) .

  In this formula, ||.|| denotes the Euclidean norm.
  */

  /**
   * Start a L-BFGS optimization.
   *
   *  @param  n           The number of variables.
   *  @param  x           The array of variables. A client program can set
   *                      default values for the optimization and receive the
   *                      optimization result through this array. This array
   *                      must be allocated by ::lbfgs_malloc function
   *                      for libLBFGS built with SSE/SSE2 optimization routine
   *                      enabled. The library built without SSE/SSE2
   *                      optimization does not have such a requirement.
   *  @param  ptr_fx      The pointer to the variable that receives the final
   *                      value of the objective function for the variables.
   *                      This argument can be set to \c NULL if the final
   *                      value of the objective function is unnecessary.
   *  @param  proc_evaluate   The callback function to provide function and
   *                          gradient evaluations given a current values of
   *                          variables. A client program must implement a
   *                          callback function compatible with \ref
   *                          lbfgs_evaluate_t and pass the pointer to the
   *                          callback function.
   *  @param  proc_progress   The callback function to receive the progress
   *                          (the number of iterations, the current value of
   *                          the objective function) of the minimization
   *                          process. This argument can be set to \c NULL if
   *                          a progress report is unnecessary.
   *  @param  instance    A user data for the client program. The callback
   *                      functions will receive the value of this argument.
   *  @param  param       The pointer to a structure representing parameters for
   *                      L-BFGS optimization. A client program can set this
   *                      parameter to \c NULL to use the default parameters.
   *                      Call lbfgs_parameter_init() function to fill a
   *                      structure with the default values.
   *  @retval int         The status code. This function returns zero if the
   *                      minimization process terminates without an error. A
   *                      non-zero value indicates an error.
   */
  int lbfgs(int n, double *x, double *ptr_fx,
    lbfgs_evaluate_t proc_evaluate, lbfgs_progress_t proc_progress,
    void *instance, lbfgs_parameter_t *param);

  /**
   * Initialize L-BFGS parameters to the default values.
   *
   *  Call this function to fill a parameter structure with the default values
   *  and overwrite parameter values if necessary.
   *
   *  @param  param       The pointer to the parameter structure.
   */
  void lbfgs_parameter_init(lbfgs_parameter_t *param);

  /**
   * Allocate an array for variables.
   *
   *  This function allocates an array of variables for the convenience of
   *  ::lbfgs function; the function has a requreiemt for a variable array
   *  when libLBFGS is built with SSE/SSE2 optimization routines. A user does
   *  not have to use this function for libLBFGS built without SSE/SSE2
   *  optimization.
   *
   *  @param  n           The number of variables.
   */
  double* lbfgs_malloc(int n);

  /**
   * Free an array of variables.
   *
   *  @param  x           The array of variables allocated by ::lbfgs_malloc
   *                      function.
   */
  void lbfgs_free(double *x);

  /*
   *  LBFGS Interface Class
   */
  class LbfgsSolver
  {
    public:
	  LbfgsSolver();
      virtual ~LbfgsSolver(){}

      bool initialize(NlpDescription* nlproblem, double constraints_weight = 1.);
      void optimize();

      bool& verbose() { return verbose_; }
      lbfgs_parameter_t& opt_params() { return opt_params_; }

    private:
      // definition of cost evaluation
      static double delegate_evaluate(void *instance, const double *x, double *g, const int n, const double step );
      double evaluate(void *instance, const double *x, double *g, const int n, const double step );
      double evaluate_objective_function(const double* eig_x);

      // definition of how to show progress of the optimization
      static int delegate_progress(void *instance, const double *x, const double *g, const double fx, const double xnorm,
  		                           const double gnorm, const double step, int n, int k, int ls );
      int progress(const double *x, const double *g, double fx, double xnorm, double gnorm, int n, int k );

    private:
      NlpDescription* nlproblem_;
      double constraints_weight_;
      bool verbose_, initialized_;
      lbfgs_parameter_t opt_params_;
      int nvars_, ncons_, *user_info_, opt_status_, length_of_address_to_this_;
  };

}
/** @} */

/**
@mainpage libLBFGS: a library of Limited-memory Broyden-Fletcher-Goldfarb-Shanno (L-BFGS)

@section intro Introduction

This library is a C port of the implementation of Limited-memory
Broyden-Fletcher-Goldfarb-Shanno (L-BFGS) method written by Jorge Nocedal.
The original FORTRAN source code is available at:
http://www.ece.northwestern.edu/~nocedal/lbfgs.html

The L-BFGS method solves the unconstrainted minimization problem,

<pre>
    minimize F(x), x = (x1, x2, ..., xN),
</pre>

only if the objective function F(x) and its gradient G(x) are computable. The
well-known Newton's method requires computation of the inverse of the hessian
matrix of the objective function. However, the computational cost for the
inverse hessian matrix is expensive especially when the objective function
takes a large number of variables. The L-BFGS method iteratively finds a
minimizer by approximating the inverse hessian matrix by information from last
m iterations. This innovation saves the memory storage and computational time
drastically for large-scaled problems.

Among the various ports of L-BFGS, this library provides several features:
- <b>Optimization with L1-norm (Orthant-Wise Limited-memory Quasi-Newton
  (OWL-QN) method)</b>:
  In addition to standard minimization problems, the library can minimize
  a function F(x) combined with L1-norm |x| of the variables,
  {F(x) + C |x|}, where C is a constant scalar parameter. This feature is
  useful for estimating parameters of sparse log-linear models (e.g.,
  logistic regression and maximum entropy) with L1-regularization (or
  Laplacian prior).
- <b>Clean C code</b>:
  Unlike C codes generated automatically by f2c (Fortran 77 into C converter),
  this port includes changes based on my interpretations, improvements,
  optimizations, and clean-ups so that the ported code would be well-suited
  for a C code. In addition to comments inherited from the original code,
  a number of comments were added through my interpretations.
- <b>Callback interface</b>:
  The library receives function and gradient values via a callback interface.
  The library also notifies the progress of the optimization by invoking a
  callback function. In the original implementation, a user had to set
  function and gradient values every time the function returns for obtaining
  updated values.
- <b>Thread safe</b>:
  The library is thread-safe, which is the secondary gain from the callback
  interface.
- <b>Cross platform.</b> The source code can be compiled on Microsoft Visual
  Studio 2010, GNU C Compiler (gcc), etc.
- <b>Configurable precision</b>: A user can choose single-precision (float)
  or double-precision (double) accuracy by changing ::LBFGS_FLOAT macro.
- <b>SSE/SSE2 optimization</b>:
  This library includes SSE/SSE2 optimization (written in compiler intrinsics)
  for vector arithmetic operations on Intel/AMD processors. The library uses
  SSE for float values and SSE2 for double values. The SSE/SSE2 optimization
  routine is disabled by default.

This library is used by:
- <a href="http://www.chokkan.org/software/crfsuite/">CRFsuite: A fast implementation of Conditional Random Fields (CRFs)</a>
- <a href="http://www.chokkan.org/software/classias/">Classias: A collection of machine-learning algorithms for classification</a>
- <a href="http://www.public.iastate.edu/~gdancik/mlegp/">mlegp: an R package for maximum likelihood estimates for Gaussian processes</a>
- <a href="http://infmath.uibk.ac.at/~matthiasf/imaging2/">imaging2: the imaging2 class library</a>

@section download Download

- <a href="https://github.com/downloads/chokkan/liblbfgs/liblbfgs-1.10.tar.gz">Source code</a>
- <a href="https://github.com/chokkan/liblbfgs">GitHub repository</a>

libLBFGS is distributed under the term of the
<a href="http://opensource.org/licenses/mit-license.php">MIT license</a>.

@section modules Third-party modules
- <a href="http://cran.r-project.org/web/packages/lbfgs/index.html">lbfgs: Limited-memory BFGS Optimization (a wrapper for R)</a> maintained by Antonio Coppola.
- <a href="http://search.cpan.org/~laye/Algorithm-LBFGS-0.16/">Algorithm::LBFGS - Perl extension for L-BFGS</a> maintained by Lei Sun.
- <a href="http://www.cs.kuleuven.be/~bernd/yap-lbfgs/">YAP-LBFGS (an interface to call libLBFGS from YAP Prolog)</a> maintained by Bernd Gutmann.

@section changelog History
- Version 1.10 (2010-12-22):
    - Fixed compiling errors on Mac OS X; this patch was kindly submitted by
      Nic Schraudolph.
    - Reduced compiling warnings on Mac OS X; this patch was kindly submitted
      by Tamas Nepusz.
    - Replaced memalign() with posix_memalign().
    - Updated solution and project files for Microsoft Visual Studio 2010.
- Version 1.9 (2010-01-29):
    - Fixed a mistake in checking the validity of the parameters "ftol" and
      "wolfe"; this was discovered by Kevin S. Van Horn.
- Version 1.8 (2009-07-13):
    - Accepted the patch submitted by Takashi Imamichi;
      the backtracking method now has three criteria for choosing the step
      length:
        - ::LBFGS_LINESEARCH_BACKTRACKING_ARMIJO: sufficient decrease (Armijo)
          condition only
        - ::LBFGS_LINESEARCH_BACKTRACKING_WOLFE: regular Wolfe condition
          (sufficient decrease condition + curvature condition)
        - ::LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE: strong Wolfe condition
    - Updated the documentation to explain the above three criteria.
- Version 1.7 (2009-02-28):
    - Improved OWL-QN routines for stability.
    - Removed the support of OWL-QN method in MoreThuente algorithm because
      it accidentally fails in early stages of iterations for some objectives.
      Because of this change, <b>the OW-LQN method must be used with the
      backtracking algorithm (::LBFGS_LINESEARCH_BACKTRACKING)</b>, or the
      library returns ::LBFGSERR_INVALID_LINESEARCH.
    - Renamed line search algorithms as follows:
        - ::LBFGS_LINESEARCH_BACKTRACKING: regular Wolfe condition.
        - ::LBFGS_LINESEARCH_BACKTRACKING_LOOSE: regular Wolfe condition.
        - ::LBFGS_LINESEARCH_BACKTRACKING_STRONG: strong Wolfe condition.
    - Source code clean-up.
- Version 1.6 (2008-11-02):
    - Improved line-search algorithm with strong Wolfe condition, which was
      contributed by Takashi Imamichi. This routine is now default for
      ::LBFGS_LINESEARCH_BACKTRACKING. The previous line search algorithm
      with regular Wolfe condition is still available as
      ::LBFGS_LINESEARCH_BACKTRACKING_LOOSE.
    - Configurable stop index for L1-norm computation. A member variable
      ::lbfgs_parameter_t::orthantwise_end was added to specify the index
      number at which the library stops computing the L1 norm of the
      variables. This is useful to prevent some variables from being
      regularized by the OW-LQN method.
    - A sample program written in C++ (sample/sample.cpp).
- Version 1.5 (2008-07-10):
    - Configurable starting index for L1-norm computation. A member variable
      ::lbfgs_parameter_t::orthantwise_start was added to specify the index
      number from which the library computes the L1 norm of the variables.
      This is useful to prevent some variables from being regularized by the
      OWL-QN method.
    - Fixed a zero-division error when the initial variables have already
      been a minimizer (reported by Takashi Imamichi). In this case, the
      library returns ::LBFGS_ALREADY_MINIMIZED status code.
    - Defined ::LBFGS_SUCCESS status code as zero; removed unused constants,
      LBFGSFALSE and LBFGSTRUE.
    - Fixed a compile error in an implicit down-cast.
- Version 1.4 (2008-04-25):
    - Configurable line search algorithms. A member variable
      ::lbfgs_parameter_t::linesearch was added to choose either MoreThuente
      method (::LBFGS_LINESEARCH_MORETHUENTE) or backtracking algorithm
      (::LBFGS_LINESEARCH_BACKTRACKING).
    - Fixed a bug: the previous version did not compute psuedo-gradients
      properly in the line search routines for OWL-QN. This bug might quit
      an iteration process too early when the OWL-QN routine was activated
      (0 < ::lbfgs_parameter_t::orthantwise_c).
    - Configure script for POSIX environments.
    - SSE/SSE2 optimizations with GCC.
    - New functions ::lbfgs_malloc and ::lbfgs_free to use SSE/SSE2 routines
      transparently. It is uncessary to use these functions for libLBFGS built
      without SSE/SSE2 routines; you can still use any memory allocators if
      SSE/SSE2 routines are disabled in libLBFGS.
- Version 1.3 (2007-12-16):
    - An API change. An argument was added to lbfgs() function to receive the
      final value of the objective function. This argument can be set to
      \c NULL if the final value is unnecessary.
    - Fixed a null-pointer bug in the sample code (reported by Takashi Imamichi).
    - Added build scripts for Microsoft Visual Studio 2005 and GCC.
    - Added README file.
- Version 1.2 (2007-12-13):
    - Fixed a serious bug in orthant-wise L-BFGS.
      An important variable was used without initialization.
- Version 1.1 (2007-12-01):
    - Implemented orthant-wise L-BFGS.
    - Implemented lbfgs_parameter_init() function.
    - Fixed several bugs.
    - API documentation.
- Version 1.0 (2007-09-20):
    - Initial release.

@section api Documentation

- @ref liblbfgs_api "libLBFGS API"

@section sample Sample code

@include sample.c

@section ack Acknowledgements

The L-BFGS algorithm is described in:
    - Jorge Nocedal.
      Updating Quasi-Newton Matrices with Limited Storage.
      <i>Mathematics of Computation</i>, Vol. 35, No. 151, pp. 773--782, 1980.
    - Dong C. Liu and Jorge Nocedal.
      On the limited memory BFGS method for large scale optimization.
      <i>Mathematical Programming</i> B, Vol. 45, No. 3, pp. 503-528, 1989.

The line search algorithms used in this implementation are described in:
    - John E. Dennis and Robert B. Schnabel.
      <i>Numerical Methods for Unconstrained Optimization and Nonlinear
      Equations</i>, Englewood Cliffs, 1983.
    - Jorge J. More and David J. Thuente.
      Line search algorithm with guaranteed sufficient decrease.
      <i>ACM Transactions on Mathematical Software (TOMS)</i>, Vol. 20, No. 3,
      pp. 286-307, 1994.

This library also implements Orthant-Wise Limited-memory Quasi-Newton (OWL-QN)
method presented in:
    - Galen Andrew and Jianfeng Gao.
      Scalable training of L1-regularized log-linear models.
      In <i>Proceedings of the 24th International Conference on Machine
      Learning (ICML 2007)</i>, pp. 33-40, 2007.

Special thanks go to:
    - Yoshimasa Tsuruoka and Daisuke Okanohara for technical information about
      OWL-QN
    - Takashi Imamichi for the useful enhancements of the backtracking method
    - Kevin S. Van Horn, Nic Schraudolph, and Tamas Nepusz for bug fixes

Finally I would like to thank the original author, Jorge Nocedal, who has been
distributing the effieicnt and explanatory implementation in an open source
licence.

@section reference Reference

- <a href="http://www.ece.northwestern.edu/~nocedal/lbfgs.html">L-BFGS</a> by Jorge Nocedal.
- <a href="http://research.microsoft.com/en-us/downloads/b1eb1016-1738-4bd5-83a9-370c9d498a03/default.aspx">Orthant-Wise Limited-memory Quasi-Newton Optimizer for L1-regularized Objectives</a> by Galen Andrew.
- <a href="http://chasen.org/~taku/software/misc/lbfgs/">C port (via f2c)</a> by Taku Kudo.
- <a href="http://www.alglib.net/optimization/lbfgs.php">C#/C++/Delphi/VisualBasic6 port</a> in ALGLIB.
- <a href="http://cctbx.sourceforge.net/">Computational Crystallography Toolbox</a> includes
  <a href="http://cctbx.sourceforge.net/current_cvs/c_plus_plus/namespacescitbx_1_1lbfgs.html">scitbx::lbfgs</a>.
*/


text:
/*  Sirikata Transfer -- Content Transfer management system
 *  HttpManager.hpp
 *
 *  Copyright (c) 2010, Jeff Terrace
 *  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 Sirikata nor the names of its contributors may
 *    be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/*  Created on: Jun 18th, 2010 */

#ifndef SIRIKATA_HttpManager_HPP__
#define SIRIKATA_HttpManager_HPP__

#include <sirikata/core/util/Platform.hpp>
#include <string>
#include <deque>
#include <sstream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/thread/mutex.hpp>
// Apple defines the check macro in AssertMacros.h, which screws up some code in boost's iostreams lib
#ifdef check
#undef check
#endif
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/copy.hpp>
#include <sirikata/core/network/IOServicePool.hpp>
#include <sirikata/core/network/IOWork.hpp>
#include <sirikata/core/network/Asio.hpp>
#include <sirikata/core/network/Address.hpp>
#include <sirikata/core/transfer/TransferData.hpp>

// This is a hack around a problem created by different packages
// creating slightly different typedefs -- notably http_parser and
// v8. On windows, we need to provide typedefs and make it skip that
// part of http_parser.h.
#if defined(_WIN32) && !defined(__MINGW32__)

typedef signed __int8 int8_t;
typedef unsigned __int8 uint8_t;
typedef __int16 int16_t;
typedef unsigned __int16 uint16_t;
typedef __int32 int32_t;
typedef unsigned __int32 uint32_t;
typedef __int64 int64_t;
typedef unsigned __int64 uint64_t;

typedef unsigned int size_t;
typedef int ssize_t;

// We turn on mingw to make it skip the include block, but then stdint
// is included, and only available via a zzip dependency. We block
// that out as well by using its include guard define
#define _ZZIP__STDINT_H 1
#define __MINGW32__
#include "http_parser.h"
#undef __MINGW32__

#else

#include "http_parser.h"

#endif

namespace Sirikata {
namespace Transfer {

/*
 * Handles managing connections to the CDN
 */
class SIRIKATA_EXPORT HttpManager
    : public AutoSingleton<HttpManager> {

protected:
	enum LAST_HEADER_CB {
		NONE,
		FIELD,
		VALUE
	};

public:
        typedef std::map<std::string, std::string> StringDictionary;
        // StringDictionary that uses case-insensitive keys, as required for
        // http headers by RFC 2616
        struct CaseInsensitiveStringLess {
            bool operator()(const std::string& lhs, const std::string& rhs) const {
                std::size_t lsize = lhs.size(), rsize = rhs.size();
                if (lsize != rsize) return (lsize < rsize);
                for(std::size_t i = 0; i < lsize; i++) {
                    char li = std::tolower(lhs[i]), ri = std::tolower(rhs[i]);
                    if (li != ri) return (li < ri);
                }
                return false;
            }
        };
        typedef std::map<std::string, std::string, CaseInsensitiveStringLess> CaseInsensitiveStringDictionary;

        typedef CaseInsensitiveStringDictionary Headers;
        typedef StringDictionary QueryParameters;

        /** Represents one field in a multipart/form-data */
        struct MultipartData {
            MultipartData(const String& _field, const String& _data)
             : field(_field), headers(), filename(""), data(_data)
            {}
            MultipartData(const String& _field, const String& _data, const String& _filename)
             : field(_field), headers(), filename(_filename), data(_data)
            {}
            MultipartData(const String& _field, const String& _data, const String& _filename, const Headers& _headers)
             : field(_field), headers(_headers), filename(_filename), data(_data)
            {}

            String field;
            Headers headers;
            String filename;
            String data;
        };
        typedef std::vector<MultipartData> MultipartDataList;

    /*
     * Stores headers and data returned from an HTTP request
     * Also stores HTTP status code and content length in numeric format
     * for convenience. These headers are also present in raw (string) format
     *
     * Note that getData might return a null pointer even if the
     * http request was successful, e.g. for a HEAD request
     *
     * Note that getContentLength might not be a valid value. If there was no
     * content length header in the response, getContentLength is undefined.
     */
    class HttpResponse {
    protected:
        // This stuff is all used internally for http-parser
        std::string mTempHeaderField;
        std::string mTempHeaderValue;
        LAST_HEADER_CB mLastCallback;
        bool mHeaderComplete;
        bool mMessageComplete;
        http_parser_settings mHttpSettings;
        http_parser mHttpParser;
        bool mGzip;
        std::stringstream mCompressedStream;
        //

        Headers mHeaders;
        std::tr1::shared_ptr<DenseData> mData;
        ssize_t mContentLength;
        unsigned short mStatusCode;

        // Stats from underlying Http Request/Response. Note that BytesReceived
        // is *not* the same as content length
        uint32 mBytesSent;
        uint32 mBytesReceived;

        HttpResponse()
            : mLastCallback(NONE), mHeaderComplete(false), mMessageComplete(false),
              mGzip(false), mContentLength(0), mStatusCode(0),
              mBytesSent(0), mBytesReceived(0)
        {}
    public:
        inline std::tr1::shared_ptr<DenseData> getData() { return mData; }
        inline const Headers& getHeaders() { return mHeaders; }
        inline StringDictionary getRawHeaders() {
            StringDictionary raw_headers;
            for(Headers::const_iterator it = mHeaders.begin(); it != mHeaders.end(); it++)
                raw_headers[it->first] = it->second;
            return raw_headers;
        }
        inline ssize_t getContentLength() { return mContentLength; }
        inline unsigned short getStatusCode() { return mStatusCode; }
        inline uint32 getBytesSent() const { return mBytesSent; }
        inline uint32 getBytesReceived() const { return mBytesReceived; }

        friend class HttpManager;
    };
    typedef std::tr1::shared_ptr<HttpResponse> HttpResponsePtr;

    //Type of errors that can be given to callback
    enum ERR_TYPE {
        SUCCESS,
        REQUEST_PARSING_FAILED,
        RESPONSE_PARSING_FAILED,
        BOOST_ERROR
    };

    /*
     * Callback for an HTTP request. If error == SUCCESS, then
     * response is an HttpResponse object that has headers and data.
     * If error == BOOST_ERROR, then boost_error is set to the boost
     * error code. If an error is returned, response is NULL.
     */
    typedef std::tr1::function<void(
                std::tr1::shared_ptr<HttpResponse> response,
                ERR_TYPE error,
                const boost::system::error_code& boost_error
            )> HttpCallback;

    static HttpManager& getSingleton();
    static void destroy();

    //Methods supported
    enum HTTP_METHOD {
        HEAD,
        GET,
        POST
    };
    static String methodAsString(HTTP_METHOD m);

    /** Makes an HTTP request and calls cb when finished. This is the lowest
     *  level version exposed publicly, taking a raw HTTP request, which you
     *  should ensure is properly formatted. Usually you should use the
     *  convenience wrappers that format the request for you.
     */
    void makeRequest(Sirikata::Network::Address addr, HTTP_METHOD method, std::string req, bool allow_redirects, HttpCallback cb);

    /** Formats and makes an HTTP request and calls cb when finished. This
     *  version is a utility for the more specific request types (i.e. head()
     *  and get()).
     *
     *  \param addr the address of the server
     *  \param method the HTTP request method, i.e. HEAD, GET, or POST
     *  \param path the path of the resource to access
     *  \param cb callback to invoke upon completion
     *  \param headers dictionary of headers to add to the request
     *  \param query_params dictionary of unencoded query parameters to add to
     *         the url
     *  \param body if non-empty, the encoded HTTP request body, i.e. the data
     *         provided after all the headers. If you provide this, you should
     *         probably include headers to specify it's format
     *  \param allow_redirects if true, redirects will be followed, triggering a
     *         new requests
     */
    void makeRequest(
        Sirikata::Network::Address addr, HTTP_METHOD method, const String& path,
        HttpCallback cb,
        const Headers& headers = Headers(), const QueryParameters& query_params = QueryParameters(),
        const String& body = "",
        bool allow_redirects = true
    );

    static String formatURLEncodedDictionary(const StringDictionary& query_params);
    static String formatPath(const String& path, const QueryParameters& query_params);
    static String formatURL(const String& host, const String& service, const String& path, const QueryParameters& query_params);

    void head(
        Sirikata::Network::Address addr, const String& path,
        HttpCallback cb, const Headers& headers = Headers(), const QueryParameters& query_params = QueryParameters(),
        bool allow_redirects = true
    );

    void get(
        Sirikata::Network::Address addr, const String& path,
        HttpCallback cb, const Headers& headers = Headers(), const QueryParameters& query_params = QueryParameters(),
        bool allow_redirects = true
    );

    /** Perform an HTTP POST using the specified content type and message
     *  body. This can be used if you want to use an unusual encoding or as a
     *  utility for other, more specific post methods.
     */
    void post(
        Sirikata::Network::Address addr, const String& path,
        const String& content_type, const String& body,
        HttpCallback cb, const Headers& headers = Headers(), const QueryParameters& query_params = QueryParameters(),
        bool allow_redirects = true
    );

    /** Perform a HTTP POST whose body is x-www-form-urlencoded parameters. */
    void postURLEncoded(
        Sirikata::Network::Address addr, const String& path,
        const StringDictionary& body,
        HttpCallback cb, const Headers& headers = Headers(), const QueryParameters& query_params = QueryParameters(),
        bool allow_redirects = true
    );

    /** Perform a HTTP POST whose body is multipart/form-data encoded body. */
    void postMultipartForm(
        Sirikata::Network::Address addr, const String& path,
        const MultipartDataList& data,
        HttpCallback cb, const Headers& headers = Headers(), const QueryParameters& query_params = QueryParameters(),
        bool allow_redirects = true
    );

protected:
    /*
     * Protect constructor and destructor so can't make an instance of this class
     * but allow AutoSingleton<HttpManager> to make a copy, and give access
     * to the destructor to the auto_ptr used by AutoSingleton
     */
    HttpManager();
    ~HttpManager();
    friend class AutoSingleton<HttpManager>;
    friend std::auto_ptr<HttpManager>::~auto_ptr();
    friend void std::auto_ptr<HttpManager>::reset(HttpManager*);

    // Formats a URL encoded dictionary -- for form-urlencoded data or query
    // strings. NOTE: There is no ? prefixed to this.
    static void formatURLEncodedDictionary(std::ostream& os, const StringDictionary& query_params);
    // Formats the entire path portion of a URL -- path + query args
    static void formatPath(std::ostream& os, const String& path, const QueryParameters& query_params);
private:
    //For convenience
    typedef Sirikata::Network::IOServicePool IOServicePool;
    typedef Sirikata::Network::TCPResolver TCPResolver;
    typedef Sirikata::Network::TCPSocket TCPSocket;
    typedef Sirikata::Network::IOWork IOWork;
    typedef Sirikata::Network::IOCallback IOCallback;
    typedef boost::asio::ip::tcp::endpoint TCPEndPoint;

    //Convenience of storing request parameters together
    class HttpRequest {
    public:
        const Sirikata::Network::Address addr;
        const std::string req;
        const HttpCallback cb;
        const HTTP_METHOD method;
        const bool allow_redirects;
        HttpRequest(Sirikata::Network::Address _addr, std::string _req, HTTP_METHOD meth, bool _allow_redirects, HttpCallback _cb)
         : addr(_addr), req(_req), cb(_cb), method(meth), allow_redirects(_allow_redirects),
           mNumTries(0), mLastCallback(NONE), mHeaderComplete(false) {}

        friend class HttpManager;
    protected:
        uint32 mNumTries;
        http_parser_settings mHttpSettings;
        http_parser mHttpParser;
        std::string mTempHeaderField;
        std::string mTempHeaderValue;
        LAST_HEADER_CB mLastCallback;
        bool mHeaderComplete;
        Headers mHeaders;
    };

    //Holds a queue of requests to be made
    typedef std::list<std::tr1::shared_ptr<HttpRequest> > RequestQueueType;
    RequestQueueType mRequestQueue;
    //Lock this to access mRequestQueue
    boost::mutex mRequestQueueLock;

    //TODO: should get these from settings
    static const uint32 MAX_CONNECTIONS_PER_ENDPOINT = 8;
    static const uint32 MAX_TOTAL_CONNECTIONS = 40;
    static const uint32 SOCKET_BUFFER_SIZE = 10240;

    //Keeps track of the total number of connections currently open
    uint32 mNumTotalConnections;

    //Keeps track of the number of connections open per host:port pair
    typedef std::map<Sirikata::Network::Address, uint32> NumConnsType;
    NumConnsType mNumConnsPerAddr;
    //Lock this to access mNumTotalConnections or mNumConnsPerAddr
    boost::mutex mNumConnsLock;

    //Holds connections that are open but not being used
    typedef std::map<Sirikata::Network::Address,
        std::queue<std::tr1::shared_ptr<TCPSocket> > > RecycleBinType;
    RecycleBinType mRecycleBin;
    //Lock this to access mRecycleBin
    boost::mutex mRecycleBinLock;

    IOServicePool* mServicePool;
    TCPResolver* mResolver;

    http_parser_settings EMPTY_PARSER_SETTINGS;

    void processQueue();

    void add_req(std::tr1::shared_ptr<HttpRequest> req);
    void decrement_connection(const Sirikata::Network::Address& addr);
    void write_request(std::tr1::shared_ptr<TCPSocket> socket, std::tr1::shared_ptr<HttpRequest> req);

    void handle_resolve(std::tr1::shared_ptr<HttpRequest> req, const boost::system::error_code& err,
            TCPResolver::iterator endpoint_iterator);
    void handle_connect(std::tr1::shared_ptr<TCPSocket> socket, std::tr1::shared_ptr<HttpRequest> req,
            const boost::system::error_code& err, TCPResolver::iterator endpoint_iterator);
    void handle_write_request(std::tr1::shared_ptr<TCPSocket> socket, std::tr1::shared_ptr<HttpRequest> req,
            const boost::system::error_code& err, std::tr1::shared_ptr<boost::asio::streambuf> request_stream);
    void handle_read(std::tr1::shared_ptr<TCPSocket> socket, std::tr1::shared_ptr<HttpRequest> req,
            std::tr1::shared_ptr<std::vector<unsigned char> > vecbuf, std::tr1::shared_ptr<HttpResponse> respPtr,
            const boost::system::error_code& err, std::size_t bytes_transferred);

    static int on_header_field(http_parser *_, const char *at, size_t len);
    static int on_header_value(http_parser *_, const char *at, size_t len);
    static int on_headers_complete(http_parser *_);
    static int on_body(http_parser *_, const char *at, size_t len);
    static int on_message_complete(http_parser *_);

    static int on_request_header_field(http_parser *_, const char *at, size_t len);
    static int on_request_header_value(http_parser *_, const char *at, size_t len);
    static int on_request_headers_complete(http_parser *_);

    enum HTTP_PARSER_FLAGS
      { F_CHUNKED = 1 << 0
      , F_CONNECTION_KEEP_ALIVE = 1 << 1
      , F_CONNECTION_CLOSE = 1 << 2
      , F_TRAILING = 1 << 3
      , F_UPGRADE = 1 << 4
      , F_SKIPBODY = 1 << 5
      };

    static void print_flags(std::tr1::shared_ptr<HttpResponse> resp);

public:

    /*
     * Posts a callback on the service pool
     */
    void postCallback(IOCallback cb, const char* tag);
    void postCallback(const Duration& waitFor, IOCallback cb, const char* tag);

};

}
}

#endif

max_stars_repo_name: sirikata/sirikata

max_issues_repo_name: sirikata/sirikata

max_forks_repo_name: sirikata/sirikata


text:
// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

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

// This library 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
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#include "rigidbodyinertia.hpp"

#include <Eigen/Core>

namespace KDL{

    const static bool mhi=true;

    RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi):
        m(m_),h(h_),I(I_)
    {
    }

    RigidBodyInertia::RigidBodyInertia(double m_, const Vector& c_, const RotationalInertia& Ic):
        m(m_),h(m*c_){
        //I=Ic-c x c x
        Eigen::Vector3d c_eig=Eigen::Map<const Eigen::Vector3d>(c_.data);
        Eigen::Map<Eigen::Matrix3d>(I.data)=Eigen::Map<const Eigen::Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Eigen::Matrix3d::Identity());
    }

    RigidBodyInertia operator*(double a,const RigidBodyInertia& I){
        return RigidBodyInertia(a*I.m,a*I.h,a*I.I,mhi);
    }

    RigidBodyInertia operator+(const RigidBodyInertia& Ia, const RigidBodyInertia& Ib){
        return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);
    }

    Wrench operator*(const RigidBodyInertia& I,const Twist& t){
        return Wrench(I.m*t.vel-I.h*t.rot,I.I*t.rot+I.h*t.vel);
    }

    RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I){
        Frame X=T.Inverse();
        //mb=ma
        //hb=R*(h-m*r)
        //Ib = R(Ia+r x h x + (h-m*r) x r x)R'
        Vector hmr = (I.h-I.m*X.p);
        Eigen::Vector3d r_eig = Eigen::Map<Eigen::Vector3d>(X.p.data);
        Eigen::Vector3d h_eig = Eigen::Map<const Eigen::Vector3d>(I.h.data);
        Eigen::Vector3d hmr_eig = Eigen::Map<Eigen::Vector3d>(hmr.data);
        Eigen::Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity();
        Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity();
        Eigen::Matrix3d R = Eigen::Map<Eigen::Matrix3d>(X.M.data);
        RotationalInertia Ib;
        Eigen::Map<Eigen::Matrix3d>(Ib.data) = R*((Eigen::Map<const Eigen::Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());

        return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi);
    }

    RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
        //mb=ma
        //hb=R*h
        //Ib = R(Ia)R' with r=0
        Eigen::Matrix3d R = Eigen::Map<const Eigen::Matrix3d>(M.data);
        RotationalInertia Ib;
        Eigen::Map<Eigen::Matrix3d>(Ib.data) = R.transpose()*(Eigen::Map<const Eigen::Matrix3d>(I.I.data)*R);

        return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
    }

    RigidBodyInertia RigidBodyInertia::RefPoint(const Vector& p){
        //mb=ma
        //hb=(h-m*r)
        //Ib = (Ia+r x h x + (h-m*r) x r x)
        Vector hmr = (this->h-this->m*p);
        Eigen::Vector3d r_eig = Eigen::Map<const Eigen::Vector3d>(p.data);
        Eigen::Vector3d h_eig = Eigen::Map<Eigen::Vector3d>(this->h.data);
        Eigen::Vector3d hmr_eig = Eigen::Map<Eigen::Vector3d>(hmr.data);
        Eigen::Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity();
        Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity();
        RotationalInertia Ib;
        Eigen::Map<Eigen::Matrix3d>(Ib.data) = Eigen::Map<Eigen::Matrix3d>(this->I.data)+rcrosshcross+hmrcrossrcross;

        return RigidBodyInertia(this->m,hmr,Ib,mhi);
    }
}//namespace

max_stars_repo_name: rocos-sia/rocos-app