S3.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>
#include <R3.h>
// #include <LogTrace.h>

using namespace std;

class Orientation3D /*: public MIPObject*/{
 
 private:
#ifdef FOUND_BOOST_SER
  friend class boost::serialization::access;
  template<class Archive>
    void serialize(Archive & ar, const unsigned int version){
   ar & _roll; 
   ar & _pitch;
   ar & _yaw;
    }
#endif
 
 protected:
  Angle _roll;
  Angle _pitch;
  Angle _yaw;

 public:
  Orientation3D() {_roll=0.0; _pitch=0.0; _yaw=0.0;}
  Orientation3D(Angle roll_in,Angle pitch_in, Angle yaw_in) {_roll=roll_in; _pitch=pitch_in; _yaw=yaw_in;}
  
  Orientation3D(Position3D vQuat,Decimal sQuat);
  
  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.dCast2Pi()/scalar;
   _pitch = _pitch.dCast2Pi()/scalar;
   _yaw   = _yaw.dCast2Pi()/scalar;
   return *this;;
  }
  
  Orientation3D operator+(const Orientation3D &other) const {
   return Orientation3D(*this) += other;
  }
  Orientation3D operator-(const Orientation3D &other) const {
   return Orientation3D(*this) -= other;
  }
  
  Orientation3D operator*(const Decimal scalar) const {
   return Orientation3D(*this) *= scalar;
  }
  Orientation3D operator/(const Decimal scalar) const {
   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) const {
   return Orientation3D(min(_roll.dCast2Pi(),p._roll.dCast2Pi()),min(_pitch.dCast2Pi(),p._pitch.dCast2Pi()),min(_yaw.dCast2Pi(),p._yaw.dCast2Pi()));
  }
  Orientation3D maximum(const Orientation3D& p) const {
   return Orientation3D(max(_roll.dCast2Pi(),p._roll.dCast2Pi()),max(_pitch.dCast2Pi(),p._pitch.dCast2Pi()),max(_yaw.dCast2Pi(),p._yaw.dCast2Pi()));
  }
  Angle roll() const {
   return _roll;
  }
  Angle pitch() const {
   return _pitch;
  }
  Angle yaw() const {
   return _yaw;
  }
  
  void unitQuat(Position3D& vQuat, Decimal& sQuat) const;
  
  void setRoll(const Angle& roll){
   _roll = roll;
  }
      
  void setPitch(const Angle& pitch){
   _pitch = pitch;
  }
    
  void setYaw(const Angle& yaw){
   _yaw = yaw;
  }
  
  vector<Decimal> vectPi() const;

  vector<Decimal> vectDeg180() const;

  vector<Decimal> vect2Pi() const;

  vector<Decimal> vectDeg360() const;
  
  string print(PrintTypes type = PRINT_TYPE_WITH_UNITS) const{
//    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.dCast2Pi() << " " << _pitch.dCast2Pi() << " " << _yaw.dCast2Pi() << " " ;
     break;
    case PRINT_TYPE_WITH_UNITS:
    default:
     s << "(" << _roll.print() << ", " << _pitch.print() << ", " << _yaw.print() << ")" ;
     break;
   }
   return s.str();
  }
  
  string toString() const{
   stringstream ss;
   ss << _roll.dCast2Pi() << "," << _pitch.dCast2Pi() << "," << _yaw.dCast2Pi();
   return ss.str();
  }
  /*
  string getObjectName() const {return string("Orientation3D");};*/
};


#endif



/* @} */