RBModel.hpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, LABUST, UNIZG-FER
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the LABUST nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Dula Nad
*  Created: 01.02.2010.
*********************************************************************/
#ifndef RBMODEL_HPP_
#define RBMODEL_HPP_
#include <labust/simulation/matrixfwd.hpp>
#include <labust/simulation/DynamicsParams.hpp>
#include <labust/simulation/NoiseModel.hpp>
#include <labust/math/NumberManipulation.hpp>
#include <labust/vehicles/ThrustAllocation.hpp>

namespace labust
{
  namespace simulation
  {
    class RBModel : public labust::simulation::DynamicsParams
    {
    public:
      enum {x=0,y,z,phi,theta,psi};
      enum {u=0,v,w,p,q,r};
      enum {X=0,Y,Z,K,M,N};
      RBModel();
      ~RBModel();

      void step(const vector& tau);

      inline void alloc_step(const vector& tauIn, vector& tauAch)
      {
        this->allocator.allocate(tauIn,tauAch);
        this->step(tauAch);
      }

      inline const vector& Eta() const {return this->eta;};
      inline const vector& Nu() const {return this->nu;};
      inline const vector& NuAcc() const {return this->nuacc;};
      inline const vector& EtaNoisy() const {return this->etaN = this->eta + this->noise.calculateW();};
      inline const vector& NuNoisy() const {return this->nuN = this->nu + this->noise.calculateV();};

      inline void setNuAndEta(const vector& nu,const vector& eta)
      {
        this->nu = this->nu0 = nu;
        this->eta = this->eta0 = eta;
      }
      inline double getPressure(double h) const{return this->rho*this->g_acc*h;};

      inline void init()
      {
        this->reset();
        calculate_mrb();
        //Calculate and set the the neutral depth where W=B
        eta(z) = (m/(2*ae*be*ce*M_PI/3*rho)-1)*ce - waterLevel - rb(z);
      }
      void reset()
      {
        this->eta = this->eta0;
        this->nu = this->nu0;
        this->B=2*labust::math::coerce((eta(z)+waterLevel+rb(z)/2)/ce+1,0,2)*M_PI/3*ae*be*ce*rho*g_acc;
      };

      double dT;
      bool isCoupled;
      double ae,be,ce;
      double waterLevel;
      vector3 current;

      vector nu0, eta0;
      mutable labust::simulation::NoiseModel noise;

      labust::vehicles::ThrustAllocator allocator;

    protected:
      void coriolis();
      void restoring_force(const matrix3& J1);
      void calculate_mrb();

      double B;
      vector nu,nuacc;
      vector eta;
      mutable vector etaN, nuN;
      vector g;
    };

    template<class Derived>
    matrix3 skewSymm3(const Eigen::MatrixBase<Derived>& vec)
    {
      matrix3 mat;
      mat<<0,-vec(2),vec(1),
                vec(2),0,-vec(0),
                -vec(1),vec(0),0;
      return mat;
    };
  }
}

/* RBMODEL_HPP_ */
#endif