Inertial.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.
 *
*/
#ifndef INERTIAL_HH
#define INERTIAL_HH

#include "msgs/msgs.h"
#include "sdf/sdf.h"
#include "math/Quaternion.hh"
#include "math/Vector3.hh"

namespace gazebo
{
  namespace physics
  {

    class Inertial
    {
      public: Inertial();
  
      public: Inertial(double mass);
  
      public: Inertial(const Inertial &_inertial);
  
      public: virtual ~Inertial();
  
      public: void Load( sdf::ElementPtr _sdf);

      public: void UpdateParameters( sdf::ElementPtr &_sdf );

      public: double GetLinearDamping();
      public: double GetAngularDamping();

      public: void SetLinearDamping(double _damping);
      public: void SetAngularDamping(double _damping);

      public: void Reset();

      public: void SetMass(double m);

      public: double GetMass() const;
  
      public: void SetInertiaMatrix( double ixx, double iyy, double izz,
                                     double ixy, double ixz, double iyz);
  
      public: void SetCoG(double cx, double cy, double cz);
  
      public: void SetCoG(const math::Vector3 &c);
  
      public: inline const math::Vector3 &GetCoG() const
              { return this->cog; }

      public: inline const math::Pose GetPose() const
              { return math::Pose(this->cog, math::Quaternion());}
  
      public: math::Vector3 GetPrincipalMoments() const;
  
      public: math::Vector3 GetProductsofInertia() const;

      public: double GetIXX() const;
      public: double GetIYY() const;
      public: double GetIZZ() const;
      public: double GetIXY() const;
      public: double GetIXZ() const;
      public: double GetIYZ() const;

      public: void SetIXX(double _v);
      public: void SetIYY(double _v);
      public: void SetIZZ(double _v);
      public: void SetIXY(double _v);
      public: void SetIXZ(double _v);
      public: void SetIYZ(double _v);
 
      public: void Rotate(const math::Quaternion &rot);
  
      public: void operator=(const Inertial &_inertial);
  
      public: Inertial operator+(const Inertial &_inertial ) const;
      public: const Inertial &operator+=(const Inertial &_inertial );

      public: void ProcessMsg(const msgs::Inertial &_msg);

      public: friend std::ostream &operator<<(std::ostream &out, 
                                              const gazebo::physics::Inertial &_inertial)
              {
                out << "Mass[" << _inertial.mass << "] CoG[" 
                    << _inertial.cog << "]";
                return out;
              }
 
      private: double mass;
      private: math::Vector3 cog;
      private: math::Vector3 principals;
      private: math::Vector3 products;
      private: sdf::ElementPtr sdf;
    };
  }
}

#endif