S3New.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



/* @{ */

#ifndef __S3_H_
#define __S3_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <stdio.h>
#include <math.h>
#include <vector>
#include <iostream>
#include <string>
#include <sstream>
#include <sys/time.h>
#include <sys/select.h>
#include <assert.h>

//BOOST SERIALIZATION
#ifdef FOUND_BOOST_SER
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#endif



#include <Types.h>
#include <Association.h>
#include <S1.h>

using namespace std;

class Orientation3D {
 
 private:
#ifdef FOUND_BOOST_SER
  friend class boost::serialization::access;
  template<class Archive>
    void serialize(Archive & ar, const unsigned int version){
   ar & _q0; 
   ar & _q1;
   ar & _q2;
   ar & _q3;
    }
#endif
 
    


 protected:
//   Angle _roll;
//   Angle _pitch;
//   Angle _yaw;
  
  bool _normalize();
  
  Decimal  _q[4];//0 scalar part, 1-3 vectorial part

 public:
  Orientation3D();
  
  Orientation3D(Position3D quatVectPart,Decimal quatScalarPart);
  
  Orientation3D(Position3D axis,Angle angle);
  
  Orientation3D& operator*=(const Orientation3D& a);
  

  const Orientation3D  operator*(const Orientation3D &other) const;
  
  Orientation3D(Angle roll_in,Angle pitch_in, Angle yaw_in);
    
  Orientation3D(const Orientation3D& p) { _roll  = p._roll; _pitch = p._pitch; _yaw   = p._yaw;}
  Orientation3D& operator=(const Orientation3D& rhs){
   if (this != &rhs){      // Not necessary in this case but it is useful to don't forget it
    _roll  = rhs._roll;
    _pitch = rhs._pitch;
    _yaw   = rhs._yaw;
   }
   return *this;
  }
  Orientation3D& operator+=(const Orientation3D& a) {
   _roll  = _roll  + a._roll;
   _pitch = _pitch + a._pitch;
   _yaw   = _yaw   + a._yaw;
   return *this;
  }
  Orientation3D& operator-=(const Orientation3D& a) {
   _roll  = _roll  - a._roll;
   _pitch = _pitch - a._pitch;
   _yaw   = _yaw   - a._yaw;
   return *this;
  }
  Orientation3D& operator*=(const Decimal scalar) {
   _roll  = _roll*scalar;
   _pitch = _pitch*scalar;
   _yaw   = _yaw*scalar;
   return *this;;
  }
  Orientation3D& operator/=(const Decimal scalar) {
   _roll  = _roll.dCast()/scalar;
   _pitch = _pitch.dCast()/scalar;
   _yaw   = _yaw.dCast()/scalar;
   return *this;;
  }
  
  const Orientation3D operator+(const Orientation3D &other) const {
   return Orientation3D(*this) += other;
  }
  const Orientation3D operator-(const Orientation3D &other) const {
   return Orientation3D(*this) -= other;
  }
  
  const Orientation3D operator*(const Decimal scalar) {
   return Orientation3D(*this) *= scalar;
  }
  const Orientation3D operator/(const Decimal scalar) {
   return Orientation3D(*this) /= scalar;
  }
  bool operator==(const Orientation3D &other) const {
   if( (_roll == other._roll) && (_pitch == other._pitch) && (_yaw == other._yaw) )
    return true;
   else return false;
  }
  bool operator!=(const Orientation3D &other) const {
   return !(*this == other);
  }
  Orientation3D minimum(const Orientation3D& p) {
   return Orientation3D(min(_roll.dCast(),p._roll.dCast()),min(_pitch.dCast(),p._pitch.dCast()),min(_yaw.dCast(),p._yaw.dCast()));
  }
  Orientation3D maximum(const Orientation3D& p) {
   return Orientation3D(max(_roll.dCast(),p._roll.dCast()),max(_pitch.dCast(),p._pitch.dCast()),max(_yaw.dCast(),p._yaw.dCast()));
  }
  Angle roll() {
   return _roll;
  }
  Angle pitch() {
   return _pitch;
  }
  Angle yaw() {
   return _yaw;
  }
  
  void setRoll(Angle roll){
   _roll = roll;
  }
      
  void setPitch(Angle pitch){
   _pitch = pitch;
  }
    
  void setYaw(Angle yaw){
   _yaw = yaw;
  }
  
  vector<Decimal> vectPi();

  vector<Decimal> vectDeg180();

  vector<Decimal> vect2Pi();

  vector<Decimal> vectDeg360();
  
  string print(PrintTypes type = PRINT_TYPE_WITH_UNITS){
//    stringstream s;
//    s.precision(9);
//    s.setf(ios::fixed,ios::floatfield);
//    s << "(" << _roll.print() << ", " << _pitch.print() << ", " << _yaw.print() << ")" ;
//    return s.str();
   stringstream s;
   s.precision(9);
   s.setf(ios::fixed,ios::floatfield);
   switch (type){
    case PRINT_TYPE_PLAIN:
     s << _roll.dCast() << " " << _pitch.dCast() << " " << _yaw.dCast() << " " ;
     break;
    case PRINT_TYPE_WITH_UNITS:
    default:
     s << "(" << _roll.print() << ", " << _pitch.print() << ", " << _yaw.print() << ")" ;
     break;
   }
   return s.str();
   
  }
};


#endif



/* @} */