Node.hpp

Go to the documentation of this file.

// This file is part of GranOO, a workbench for DEM simulation.
//
// Author(s)    : - Damien Andre       IRCER/UNILIM, Limoges France
//                  <damien.andre@unilim.fr>
//                - Jean-luc Charles   Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <jean-luc.charles@ensam.eu>
//                - Jeremie Girardot   Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <jeremie.girardot@ensam.eu>
//                - Cedric Hubert      LAMIH/UPHF, Valenciennes France
//                  <cedric.hubert@uphf.fr>
//                - Ivan Iordanoff     Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <ivan.iordanoff@ensam.eu>
//
// Copyright (C) 2008-2019 D. Andre, JL. Charles, J. Girardot, C. Hubert, I. Iordanoff
//
// This program 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.
//
// This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
 
 
#ifndef _LibPhysic_Node
#define _LibPhysic_Node
 
 
#include "GranOO3/Core/Base.hpp"
#include "GranOO3/Physic/ExtendedSetOf/Node.hpp"
 
#include "GranOO3/Core/MArray.hpp"
#include "GranOO3/Core/PropClass.hpp"
#include "GranOO3/Physic/Dof.hpp"
#include "GranOO3/Geom/Point.hpp"
#include "GranOO3/Geom/Vector.hpp"
#include "GranOO3/Geom/EulerAngle.hpp"
#include "GranOO3/Geom/Frame.hpp"
#include "GranOO3/Geom/Quaternion.hpp"
 
 
namespace GranOO3
{
  namespace FEM
  {
    class Surface;
  }
 
  namespace Physic
  {
    class Ground;
 
    class Node : public Core::Base, public Core::PropClass<Node>, public Core::Register<Node> {
 
      GRANOO_CLASS(Physic::Node, Core::Base);
 
    public:
      // Constructors and destructor
      Node(const Geom::Vector &position);
      Node(const Geom::Point &position);
      Node();
 
      virtual ~Node();
 
      void clear_force();
      void clear_torque();
 
      // configuration
      #ifdef FEM_MODE
      GRANOO_ACCESS_GET(dof, Physic::Dof<6>, _dof);
      GRANOO_ACCESS_REF(dof, Physic::Dof<6>, _dof);
      #endif
 
      // position
      GRANOO_ACCESS(initial_position, Geom::Vector , _initial_position);
      GRANOO_ACCESS_GET(position    , Geom::Vector , _position        ); // the same data in memory !
      GRANOO_ACCESS_GET(center      , Geom::Point  , _center          ); // note that center and position are
 
      // linear
      
      GRANOO_ACCESS(linear_velocity    , Geom::Vector    , _linear_velocity    );
      GRANOO_ACCESS(linear_acceleration, Geom::Vector    , _linear_acceleration);
      
 
      // angular
      GRANOO_ACCESS(initial_quaternion     , Geom::Quaternion, _initial_quat);
      GRANOO_ACCESS(quaternion             , Geom::Quaternion, _quat        );
      
      GRANOO_ACCESS(angular_velocity       , Geom::Vector    , _angular_vel );
      GRANOO_ACCESS(quaternion_velocity    , Geom::Quaternion, _quat_vel    );
      GRANOO_ACCESS(quaternion_acceleration, Geom::Quaternion, _quat_acc    );
      
 
      
      #ifdef FEM_MODE
      GRANOO_ACCESS_GET(displacement   , Geom::Vector    , *_displacement  );
      GRANOO_ACCESS(force              , Geom::Vector    , *_force         );
      GRANOO_ACCESS(angle              , Geom::EulerAngle, *_angle         );
      GRANOO_ACCESS(torque             , Geom::Vector    , *_torque        );
      #else
      GRANOO_ACCESS_GET(displacement   , Geom::Vector    , _displacement  );
      GRANOO_ACCESS(force              , Geom::Vector    , _force         );
      GRANOO_ACCESS(angle              , Geom::EulerAngle, _angle         );
      GRANOO_ACCESS(torque             , Geom::Vector    , _torque        );
      #endif
      
      const Geom::Frame& local_frame() const;
 
      // Memory management
      #ifdef FEM_MODE
      Core::MPtr<Geom::Vector>    & displacement_ptr() { return _displacement; };
      Core::MPtr<Geom::Vector>    & force_ptr()        { return _force; };
      Core::MPtr<Geom::Vector>    & torque_ptr()       { return _torque; };
      Core::MPtr<Geom::EulerAngle>& angle_ptr()        { return _angle; };
      #endif
      
      // Movement management
      void incremental_move(const Geom::Vector& vec); 
      void incremental_unmove(const Geom::Vector& vec);
      void update_position_from_displacement();
      void update_displacement_from_position();
      void set_displacement(const Geom::Vector&);
      void clear_displacement();
      void set_displacement_x(const double&);
      void set_displacement_y(const double&);
      void set_displacement_z(const double&);
      void set_position(const Geom::Vector&);
      void set_position_x(const double&);
      void set_position_y(const double&);
      void set_position_z(const double&);
      void set_center  (const Geom::Point&);
      void update_initial_parameter_to_current_parameter();
 
      void update_from_fem();
      void update_from_dem();
 
      void add_interaction(Node&);
      void erase_interaction(Node&);
      bool interact_with(const Node &) const;
      bool interact_with(const Ground &) const; // always return false
      bool interact_with(const FEM::Surface &) const;
 
 
      //FORCE COMPUTATION
      void apply_force (const Geom::Vector&);
      void apply_torque(const Geom::Vector&);
 
      // KINEMATICS
      void set_quaternion_velocity_from_angular_velocity(const Geom::Vector& angularVelocity);
      void set_quaternion_acceleration_from_angular_acceleration(const Geom::Vector& angularAcceleration, const Geom::Vector& angularVelocity);
      void update_angular_velocity();
      virtual void clear_kinematic();
      Geom::Vector get_linear_velocity_at(const Geom::Point&) const;
      
      // MASS
      virtual double get_mass() const;
 
      // CLONING
      void make_equal_to(const Node&);
 
      // IO
      virtual std::string info() const;
      virtual std::ostream& write_ascii (std::ostream& out) const;
      virtual std::istream& read_ascii (std::istream& in);
      virtual std::ostream& export_to_povray (std::ostream& out) const;
      virtual void draw();
      
    private:
      Node(const Node&) = delete;
      Node & operator=(const Node &) = delete;
 
      //SERIALIZATION
      friend class boost::serialization::access;
      template<class Archive> void save(Archive&, const unsigned int ) const;
      template<class Archive> void load(Archive&, const unsigned int);
      BOOST_SERIALIZATION_SPLIT_MEMBER();
      
    private:
      Core::SetOfBase<Node>        _interaction;
      
      Geom::Vector                 _initial_position;
      Geom::Point                  _center;
      Geom::Vector&                _position;
 
      #ifdef FEM_MODE
      Core::MPtr<Geom::Vector>     _displacement;
      Core::MPtr<Geom::Vector>     _force;
      #else
      Geom::Vector                 _displacement;
      Geom::Vector                 _force;
      #endif
      
      Geom::Vector                 _linear_velocity;
      Geom::Vector                 _linear_acceleration;
 
      #ifdef FEM_MODE
      Core::MPtr<Geom::Vector>     _torque;
      #else
      Geom::Vector                 _torque;
      #endif
      Geom::Quaternion             _initial_quat;
      Geom::Quaternion             _quat;
      #ifdef FEM_MODE
      Core::MPtr<Geom::EulerAngle> _angle;
      #else
      Geom::EulerAngle              _angle;
      #endif
      Geom::Vector                 _angular_vel;
      Geom::Quaternion             _quat_vel;
      Geom::Quaternion             _quat_acc;
 
      const Geom::Frame            _frame;
#ifdef FEM_MODE
      Dof<6>                       _dof;
#endif
    };
 
    inline
    Node::Node() : Node(Geom::Vector(0, 0, 0)) {
    }
 
    inline
    Node::Node(const Geom::Point &position) : Node(position.to_vector()) {
    }
 
    inline
    Node::Node(const Geom::Vector &position) :
      _interaction(),
      _initial_position(position),
      _center(position.to_point()),
      _position(_center.to_vector()),
      _displacement(0.,0.,0.),
      _force(0.,0.,0.),
      _linear_velocity(0.,0.,0.),
      _linear_acceleration(0.,0.,0.),
      _torque(0.,0.,0.),
      _initial_quat(0.,0.,0.,1.),
      _quat(0.,0.,0.,1.),
      _angle(0.,0.,0.),
      _angular_vel(0.,0.,0.),      
      _quat_vel(0.,0.,0.,0.),
      _quat_acc(0.,0.,0.,0.),
      _frame(_center, _quat, Geom::Frame::global)
#ifdef FEM_MODE
      ,_dof((const double*&)(_displacement.ptr()))
#endif
    {
 
    }
 
    inline
    Node::~Node() {
 
    }
 
    inline const Geom::Frame &
    Node::local_frame() const {
 
      return _frame;
    }
    
    inline void
    Node::clear_force() {
      #ifdef FEM_MODE
      _force->clear();
      #else
      _force.clear();
      #endif
    }
 
    inline void
    Node::clear_torque() {
      #ifdef FEM_MODE
      _torque->clear();
      #else
      _torque.clear();
      #endif
    }
 
    inline void
    Node::incremental_move(const Geom::Vector& v) {
 
      _displacement += v;
      _position    += v;
    }
 
    inline void
    Node::incremental_unmove(const Geom::Vector& v) {
 
      _displacement -= v;
      _position     -= v;
    }
    
   
    inline void
    Node::set_displacement(const Geom::Vector& v) {
 
      _displacement = v;
      _position = get_initial_position() + v;
    }
    
    inline void
    Node::clear_displacement() {
#ifdef FEM_MODE
      _displacement->clear();
#else
       _displacement.clear();
#endif
      _position = _initial_position;
    }
    
    inline void
    Node::set_displacement_x(const double& val) {
#ifdef FEM_MODE
      _displacement->x() = val;
#else
      _displacement.x() = val;
#endif
      _position.x()      = _initial_position.x() + val;
    }
    
    inline void
    Node::set_displacement_y(const double& val) {
#ifdef FEM_MODE
      _displacement->y() = val;
#else
      _displacement.y() = val;
#endif
      _position.y()      = _initial_position.y() + val;
    }
    
    inline void
    Node::set_displacement_z(const double& val) {
#ifdef FEM_MODE
      _displacement->z() = val;
#else
      _displacement.z() = val;
#endif
      _position.z()      = _initial_position.z() + val;
    }
 
    inline void
    Node::set_position(const Geom::Vector& v) {
      _position     = v;
      _displacement = _position - _initial_position;
    }
 
    inline void
    Node::set_position_x(const double& d) {
      _position.x()      = d;
#ifdef FEM_MODE
      _displacement->x() = _position.x() - _initial_position.x(); 
#else
      _displacement.x() = _position.x() - _initial_position.x(); 
#endif
      
    }
 
    inline void
    Node::set_position_y(const double& d) {
      _position.y()      = d;
#ifdef FEM_MODE
      _displacement->y() = _position.x() - _initial_position.x(); 
#else
      _displacement.y() = _position.x() - _initial_position.x(); 
#endif
    }
 
    inline void
    Node::set_position_z(const double& d) {
      _position.z()      = d;
#ifdef FEM_MODE
      _displacement->z() = _position.x() - _initial_position.x(); 
#else
      _displacement.z() = _position.x() - _initial_position.x(); 
#endif
    }
    
    inline void
    Node::set_center(const Geom::Point& p) {
      _center       = p;
      _displacement = _position - _initial_position;
    }
 
    
    inline void
    Node::update_position_from_displacement() {
 
      _position = initial_position() + _displacement;
    }
 
    inline void
    Node::update_displacement_from_position() {
 
      _displacement = _position - _initial_position;
    }
    
    inline void
    Node::update_initial_parameter_to_current_parameter() {
 
      _initial_position   = _position;
      _initial_quat       = _quat;
#ifdef FEM_MODE
      _displacement->clear();
#else
      _displacement.clear();
#endif
    }
 
    inline void
    Node::update_from_fem() {
#ifdef FEM_MODE
      _angle->to_quaternion(_quat);
#else
      _angle.to_quaternion(_quat);
#endif
      update_position_from_displacement();
    }
 
    inline void
    Node::update_from_dem() {
#ifdef FEM_MODE
      _quat.to_euler_angle(*_angle);
#else
      _quat.to_euler_angle(_angle);
#endif
    }
 
    inline void
    Node::clear_kinematic() {
      _linear_velocity.clear();
      _linear_acceleration.clear();
      _quat_vel.clear();
      _quat_acc.clear();
    }
 
    inline void
    Node::set_quaternion_velocity_from_angular_velocity(const Geom::Vector& angVel) {
      // Note that the quaternion position must be updated before calling this function
      _quat_vel = 0.5 * Geom::Quaternion(angVel)*_quat;
    }
 
    inline void
    Node::set_quaternion_acceleration_from_angular_acceleration(const Geom::Vector& angAcc, const Geom::Vector& angVel) {
      _quat_acc = 0.5 * Geom::Quaternion(angAcc)*quaternion()+0.5 * Geom::Quaternion(angVel)*_quat_vel;
    }
 
    inline void
    Node::update_angular_velocity() {
      const Geom::Quaternion   &  q = quaternion();
      const Geom::Quaternion   & dq = quaternion_velocity();
      const Geom::Vector w = (2*(q.conjugate()*dq)).to_vector();
      angular_velocity() = Geom::Vector(w, local_frame(), Geom::Frame::global);
    }
 
    inline void
    Node::apply_force(const Geom::Vector& f) {
      force() += f;
    }
 
    inline void
    Node::apply_torque(const Geom::Vector& t) {
      torque() += t;
    }
 
 
    inline bool
    Node::interact_with(const Ground &) const {
      return false;
    }
 
    inline bool
    Node::interact_with(const FEM::Surface &) const {
      return false;
    }
 
 
    template<class Archive> void
    Node::save(Archive& ar, const unsigned int v) const {
      ar << boost::serialization::base_object<Core::Base>(*this);
 
      ar << _initial_position;
      ar << _center;
      ar << _displacement;
      ar << _linear_velocity;
      ar << _linear_acceleration;
      ar << _force;
 
      ar << _initial_quat;
      ar << _quat;
      ar << _angle;
      ar << _quat_vel;
      ar << _quat_acc;
      ar << _torque;
      
    }
 
    template<class Archive> void
    Node::load(Archive& ar, const unsigned int v) {
      ar >> boost::serialization::base_object<Core::Base>(*this);
 
      ar >> _initial_position;
      ar >> _center;
      ar >> _displacement;
      ar >> _linear_velocity;
      ar >> _linear_acceleration;
      ar >> _force;
 
      ar >> _initial_quat;
      ar >> _quat;
      ar >> _angle;
      ar >> _quat_vel;
      ar >> _quat_acc;
      ar >> _torque;
      update_angular_velocity();
    }
 
  }
  GRANOO_CLASS_DECLARE_TPL(Physic::Node);
}
 
#endif