Pose.hh

/*
 * Copyright 2011 Nate Koenig & Andrew Howard
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: External interfaces for Gazebo
 * Author: Nate Koenig
 * Date: 03 Apr 2007
 */

#ifndef POSE_HH
#define POSE_HH

#include <iostream>

#include "math/Vector3.hh"
#include "math/Quaternion.hh"

namespace gazebo
{
    namespace math
  {
    
    class Pose
    {
      public: Pose();
    
      public: Pose(const Vector3 &pos, const Quaternion &rot);
    
      public: Pose( const Pose &pose );
    
      public: virtual ~Pose();

      public: void Set(const Vector3 &_pos, const Quaternion &_rot);

      public: bool IsFinite() const;
    
      public: inline void Correct()
              {  this->pos.Correct(); this->rot.Correct(); }

    
      public: Pose GetInverse() const;
    
      public: Pose operator+(const Pose &pose) const;
    
      public: const Pose &operator+=(const Pose &pose);
    
      public: inline Pose operator-(const Pose &_pose) const
              {
                return Pose(this->CoordPositionSub(_pose), 
                            this->CoordRotationSub(_pose.rot));
              }

      public: const Pose &operator-=(const Pose &_pose);

      public: bool operator==(const Pose &_pose) const;

      public: bool operator!=(const Pose &_pose) const;
    
      public: Pose operator*(const Pose &pose);
              
      public: Vector3 CoordPositionAdd(const Vector3 &pos) const;
    
      public: Vector3 CoordPositionAdd(const Pose &pose) const;
    
      public: inline Vector3 CoordPositionSub(const Pose &_pose) const
              {
                Quaternion tmp( 0.0,
                    this->pos.x - _pose.pos.x,
                    this->pos.y - _pose.pos.y,
                    this->pos.z - _pose.pos.z);

                tmp = _pose.rot.GetInverse() * (tmp * _pose.rot);
                return Vector3(tmp.x, tmp.y, tmp.z);
              }
    
      public: Quaternion CoordRotationAdd(const Quaternion &rot) const;
    
      public: inline Quaternion CoordRotationSub(const Quaternion &rot) const
              {
                Quaternion result(rot.GetInverse() * this->rot);
                result.Normalize();
                return result;
              }

    
      public: Pose CoordPoseSolve(const Pose &b) const;
    
      public: void Reset();
    
      public: Pose RotatePositionAboutOrigin(const Quaternion &rot) const;

      public: void Round(int _precision);
    
      public: Vector3 pos;
    
      public: Quaternion rot;
    
      public: friend std::ostream &operator<<(std::ostream &out, 
                                              const gazebo::math::Pose &pose)
              {
                out << pose.pos << " " << pose.rot;
                return out;
              }

    public: friend std::istream &operator>>( std::istream &in, 
                gazebo::math::Pose &pose )
            {
              // Skip white spaces
              in.setf( std::ios_base::skipws );
              in >> pose.pos >> pose.rot;
              return in;
            }
    };
    
  }

}
#endif