privatepgtypes.cpp

/*
 * Copyright 2005, 2006, 2007, 2008, 2009, 2010, 
 *
 * Andrei Herdt
 * Olivier Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl 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 3 of the License, or
 * (at your option) any later version.
 *
 * walkGenJrl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  Research carried out within the scope of the 
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
#include <privatepgtypes.hh>

#include <iostream>
#include <fstream>

namespace PatternGeneratorJRL
{

  struct support_state_t & support_state_t::operator =(const support_state_t & aSS)
  {

    Phase  = aSS.Phase;
    Foot  = aSS.Foot;
    NbStepsLeft  = aSS.NbStepsLeft;
    TimeLimit = aSS.TimeLimit;
    StepNumber  = aSS.StepNumber;
    StateChanged = aSS.StateChanged;
    StartTime = aSS.StartTime;
    X = aSS.X;
    Y = aSS.Y;
    Yaw = aSS.Yaw;

    return *this;

  }


  void support_state_t::reset()
  {

    Phase = DS;
    Foot = LEFT;
    NbStepsLeft  = 0;
    TimeLimit = 0.0;
    StepNumber  = 0;
    StateChanged = false;
    NbInstants = 0;
    StartTime = 0.0;
    X = 0.0;
    Y = 0.0;
    Yaw = 0.0;

  }


  support_state_t::support_state_t()
  {

    reset();

  }


  struct com_t & com_t::operator=(const com_t &aCS)
  {

    for(unsigned int i=0;i<3;i++)
      {
	x[i] = aCS.x[i];
	y[i] = aCS.y[i];
	z[i] = aCS.z[i];
      };
    return *this;

  }
      

  void com_t::reset()
  {

    x.resize(3,false);
    y.resize(3,false);
    z.resize(3,false);
    x.clear();
    y.clear();
    z.clear();

  }


  com_t::com_t()
  {

    reset();

  }


  struct trunk_t & trunk_t::operator=(const trunk_t &aTS)
  {

    for(unsigned int i=0;i<3;i++)
      {
        x[i] = aTS.x[i];
        y[i] = aTS.y[i];
        z[i] = aTS.z[i];

        yaw[i] = aTS.yaw[i];
        pitch[i] = aTS.pitch[i];
        roll[i] = aTS.roll[i];
      };
    return *this;

  }


  void trunk_t::reset()
  {

    x.resize(3,false);
    y.resize(3,false);
    z.resize(3,false);
    yaw.resize(3,false);
    pitch.resize(3,false);
    roll.resize(3,false);
    x.clear();
    y.clear();
    z.clear();
    yaw.clear();
    pitch.clear();
    roll.clear();

  }

  trunk_t::trunk_t()
  {

    reset();

  }


  void
  convex_hull_t::rotate( axis_e axis, double angle )
  {

    switch(axis)
    {
    case YAW:
      double xOld, yOld;
      for( unsigned int j=0; j<X_vec.size(); j++ )
        {
          xOld = X_vec[j];
          yOld = Y_vec[j];
          X_vec[j] = ( xOld*cos(angle) - yOld*sin(angle) );
          Y_vec[j] = ( xOld*sin(angle) + yOld*cos(angle) );
        }
      break;
    case PITCH:
      break;
    case ROLL:
      break;
    case X_AXIS:
      break;
    case  Y_AXIS:
      break;
    case Z_AXIS:
      break;
    }

  }


  convex_hull_t::convex_hull_t( unsigned nbVert, unsigned nbIneq ):
      nbIneq_(0), nbVert_(0)
  {
    clear();
    resize( nbVert, nbIneq );
  }


  void
  convex_hull_t::clear()
  {

    X_vec.clear();
    Y_vec.clear();
    Z_vec.clear();
    A_vec.clear();
    B_vec.clear();
    C_vec.clear();
    D_vec.clear();

  }


  void
  convex_hull_t::resize( unsigned nbVert, unsigned nbIneq )
  {

    X_vec.resize(nbVert);
    Y_vec.resize(nbVert);
    Z_vec.resize(nbVert);
    A_vec.resize(nbIneq);
    B_vec.resize(nbIneq);
    C_vec.resize(nbIneq);
    D_vec.resize(nbIneq);

    nbVert_ = nbVert;
    nbIneq_ = nbIneq;

  }


  void
  convex_hull_t::set_vertices( const double * X_a, const double * Y_a, const double * Z_a )
  {

    for(unsigned i=0; i<nbVert_; i++)
      {
        X_vec[i] = X_a[i];
        Y_vec[i] = Y_a[i];
        Z_vec[i] = Z_a[i];
      }

  }
  void
  convex_hull_t::set_vertices( const double * X_a, const double * Y_a )
  {

    for(unsigned i=0; i<nbVert_; i++)
      {
        X_vec[i] = X_a[i];
        Y_vec[i] = Y_a[i];
      }

  }
  void
  convex_hull_t::set_inequalities( const double * A_a, const double * B_a, const double * C_a, const double * D_a )
  {

    for(unsigned i=0; i<nbIneq_; i++)
      {
        A_vec[i] = A_a[i];
        B_vec[i] = B_a[i];
        C_vec[i] = C_a[i];
        D_vec[i] = D_a[i];
      }

  }

  void
  convex_hull_t::cout()
  {

    std::cout<<"Vertices: "<<nbVert_<<std::endl;
    for(unsigned i=0; i<nbVert_; ++i)
      std::cout<<X_vec[i]<<" "<<Y_vec[i]<<std::endl;
    std::cout<<std::endl;

    std::cout<<"Inequalities: "<<nbIneq_<<std::endl;
    for(unsigned i=0; i<nbIneq_; ++i)
      std::cout<<A_vec[i]<<" "<<B_vec[i]<<" "<<C_vec[i]<<" "<<D_vec[i]<<std::endl;
    std::cout<<std::endl;

  }


  void
  linear_inequality_t::clear()
  {

    D.X_mat.clear();
    D.Y_mat.clear();
    D.Z_mat.clear();
    Dc_vec.clear();

  }


  void
  linear_inequality_t::resize( int NbRows, int NbCols, bool Preserve )
  {

    D.X_mat.resize(NbRows, NbCols, Preserve);
    D.Y_mat.resize(NbRows, NbCols, Preserve);
    D.Z_mat.resize(NbRows, NbCols, Preserve);
    Dc_vec.resize(NbRows, Preserve);

  }


  solution_t::solution_t():
      NbVariables(0),NbConstraints(0),Fail(0),Print(0),
      Solution_vec(0),SupportOrientations_deq(0),SupportStates_deq(0),
      ConstrLagr_vec(0),LBoundsLagr_vec(0),UBoundsLagr_vec(0)
  {  }


  void
  solution_t::reset()
  {

    NbVariables =       0;
    NbConstraints =     0;
    Fail =              0;
    Print =             0;

    Solution_vec.resize                 (0,false);
    SupportOrientations_deq.resize      (0);
    TrunkOrientations_deq.resize        (0);
    SupportStates_deq.resize            (0);
    ConstrLagr_vec.resize               (0,false);
    LBoundsLagr_vec.resize              (0,false);
    UBoundsLagr_vec.resize              (0,false);

  }


  void
  solution_t::resize( unsigned int SizeSolution, unsigned int SizeConstraints )
  {
    NbVariables = SizeSolution;
    NbConstraints = SizeConstraints;

    Solution_vec.resize(SizeSolution, false);
    ConstrLagr_vec.resize(SizeConstraints, false);
    LBoundsLagr_vec.resize(SizeSolution, false);
    UBoundsLagr_vec.resize(SizeSolution, false);
  }


  void
  solution_t::dump( const char * FileName )
  {
    std::ofstream aof;
    aof.open(FileName,std::ofstream::out);
    print(aof);
    aof.close();
  }


  void
  solution_t::print( std::ostream & aos )
  {
    aos << "Arrays:" << std::endl
        << "Solution: ";
    for( unsigned int i = 0; i < NbVariables; i++ )
      {aos<<Solution_vec[i]<<" ";}; aos<<std::endl;
  }

  reference_t::reference_t():Global(), Local()
    {    }

  reference_t::reference_t(const reference_t & R):Global(R.Global), Local(R.Local)
    {    }

  reference_t::frame_t::frame_t(): X(0), Y(0), Yaw(0), X_vec(), Y_vec(), Yaw_vec()
    {   }

  reference_t::frame_t::frame_t(const frame_t & F):
      X(F.X), Y(F.Y), Yaw(F.Yaw), X_vec(F.X_vec), Y_vec(F.Y_vec), Yaw_vec(F.Yaw_vec)
  {}

}