RigidBody.h

/****************************************************************************
 * Copyright 2005 Evan Drumwright
 * This library is distributed under the terms of the GNU General Public 
 * License (found in COPYING).
 ****************************************************************************/

#ifndef _RIGID_BODY_H
#define _RIGID_BODY_H

#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <Physsim/Constants.h>
#include <Physsim/CollisionGeometry.h>
#include <Physsim/DynamicBody.h>
#include <Physsim/Vector3.h>
#include <Physsim/Matrix3.h>
#include <Physsim/Matrix4.h>
#include <Physsim/Quat.h>
#include <Physsim/Vector3.h>
#include <Physsim/SVector6.h>

namespace Physsim {

class ArticulatedBody;
class CollisionGeometry;


class RigidBody : public DynamicBody
{
        public:
                RigidBody();
                virtual ~RigidBody() {}
                virtual void load_state(BaseConstPtr clone, bimap<BasePtr, BasePtr>* correspondence = NULL);
                virtual bool is_struct_identical(BaseConstPtr object) const;
                virtual void save_state(BasePtr object, bimap<BasePtr, BasePtr>* correspondence = NULL) const;
                virtual void clone(BasePtr& cloned, bimap<BasePtr, BasePtr>* obj_map = NULL) const;
                void set_avel(const Vector3& avel, bool update_angular_momentum = false);
                void set_qvel(const Quat& qd, bool update_angular_momentum = false);
                void set_lvel(const Vector3& lvel, bool update_linear_momentum = false);
                virtual VectorN get_state_deriv();
                virtual void set_state(const VectorN& state);
                virtual VectorN get_state();
                void add_force(const Vector3& force);
                void add_torque(const Vector3& torque);
                void set_transform(const Matrix4& transform);
                void set_transform(const Quat& q, const Vector3& x);
                void set_inertia(const Matrix3& m);
                void set_enabled(bool flag);
                void set_orientation(const Quat& q);
                void set_position(const Vector3& pos);
                void remove_collision_geometry(CollisionGeometryPtr g);
                void add_force(const Vector3& f, const Vector3& p);
                void apply_impulse(const Vector3& j, const Vector3& p);
                void apply_impulse(const Vector3& j, const Vector3& k, const Vector3& p);       
                void set_mass(Real mass);
                Matrix3 det_collision_matrix(const Matrix3& contact_frame_transpose, const Vector3& contact_point);
                virtual void transform(const Matrix4& transform) { set_transform(transform * _F); }
                virtual void calc_fwd_dyn();
                std::list<CollisionGeometryPtr> get_all_collision_geometries() const;
                void set_linear_momentum(const Vector3& P, bool update_velocity = false);
                void set_angular_momentum(const Vector3& L, bool update_velocity = false);
                virtual void set_visualization_data(SoNode* vdata) { Visualizable::set_visualization_data(vdata); synchronize(); }
                virtual void load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map);
                virtual void save_to_xml(XMLTreePtr node, std::list<BaseConstPtr>& shared_objects) const;
                Real calc_point_accel(const Vector3& point, const Vector3& dir);
                static Real calc_sep_accel(RigidBody& rb1, RigidBody& rb2, const Vector3& point, const Vector3& dir, const Vector3& dir_dot);
                Real calc_point_vel(const Vector3& point, const Vector3& dir) const;
                virtual void output_object_state(std::ostream& out) const;
                bool remove_child_link(RigidBodyConstPtr link);
                Vector3 get_inner_joint_to_com_vector_global() const;
                void set_inner_joint_to_com_vector_link(const Vector3& v);
                void set_inner_joint_to_com_vector_global(const Vector3& v);
                const Vector3& get_com_to_outer_joint_vector_link(RigidBodyConstPtr child) const;
                Vector3 get_com_to_outer_joint_vector_global(RigidBodyConstPtr child) const;
                void add_child_link_global(RigidBodyPtr child, const Vector3& v);
                bool is_child_link(RigidBodyConstPtr query) const;
                void reset_spatial_transforms();
                const SMatrix6& get_spatial_transform_forward();
                const SMatrix6& get_spatial_transform_backward();
                const SMatrix6& get_spatial_transform_link_to_global();
                const SMatrix6& get_spatial_transform_global_to_link();
                SVector6 get_spatial_accel(ReferenceFrameType rftype);
                void set_spatial_accel(const SVector6& a, ReferenceFrameType rftype);
                SVector6 get_spatial_velocity(ReferenceFrameType rftype);
                void set_spatial_velocity(const SVector6& v, ReferenceFrameType rftype);
                std::list<RigidBodyPtr> get_child_links() const;
                void add_child_link_link(RigidBodyPtr l, const Vector3& com_to_child_inner_joint_vec_link);
                void set_inner_joint(JointPtr j);
                void set_parent_link(RigidBodyPtr l);

                RigidBodyPtr get_this() { return boost::dynamic_pointer_cast<RigidBody>(shared_from_this()); }
        
                RigidBodyConstPtr get_this() const { return boost::dynamic_pointer_cast<const RigidBody>(shared_from_this()); }

                const Matrix4& get_transform() const { return _F; }

                Real get_mass() const { return _mass; }
                
                Real get_mass_inv() const { return _mass_inv; }
                
                const Matrix3& get_inertia() const { return _I; }

                const Matrix3& get_inertia_inv() const { return _Iinv; }

                const Vector3& get_position() const { return _x; }
                
                const Quat& get_orientation() const { return _q; }
                
                const Vector3& get_lvel() const { return _xd; }
                
                const Vector3& get_avel() const { return _omega; }
                
                const Quat& get_qvel() const { return _qd; }
                

                const Vector3& get_laccel() { return _a; }


                const Vector3& get_aaccel() { return _alpha; }

                void set_aaccel(const Vector3& a) { _alpha = a; }

                void set_laccel(const Vector3& a) { _a = a; }

                const SMatrix6& get_spatial_iso_inertia_link() { return _Ii_iso; }
                
                void reset_accumulators() { _forces = ZEROS_3; _torques = ZEROS_3; }
                

                const Vector3& sum_forces() const { return _forces; }
                

                const Vector3& sum_torques() const { return _torques; }

                bool is_enabled() const { return _enabled; }
                
                unsigned num_collision_geometries() const { return _geometries.size(); }
                
                void add_collision_geometry(CollisionGeometryPtr g) { g->set_rigid_body(get_this()); _geometries.push_back(g); synchronize(); }
                
                void remove_collision_geometry(unsigned i) { _geometries[i] = _geometries.back(); _geometries.pop_back(); }
                
                void clear_collision_geometries() { _geometries.clear(); }
                
                CollisionGeometryPtr get_collision_geometry(unsigned i) { return _geometries[i]; }
                
                const std::vector<CollisionGeometryPtr>& get_collision_geometries() const { return _geometries; }
                
                const Vector3& get_linear_momentum() const { return _P; }

                void update_linear_momentum() { _P = _mass * _xd; }
                
                const Vector3& get_angular_momentum() const { return _L; }

                void update_angular_momentum() { _L = _I * _omega; }


                boost::weak_ptr<ArticulatedBody> get_articulated_body() const { return _abody; }


                void set_articulated_body(boost::weak_ptr<ArticulatedBody> body) { _abody = body; }

                const std::map<RigidBodyPtr, Vector3>& get_child_link_map() const { return _child_links; }

                bool is_base() const { assert(!_abody.expired()); return _parent_link.expired(); }
                        

                const Vector3& get_inner_joint_to_com_vector_link() const { return _d; }

                boost::weak_ptr<Joint> get_inner_joint() const { return _inner_joint; }
                
                boost::weak_ptr<RigidBody> get_parent_link() const { return _parent_link; }

                unsigned num_child_links() const { return _child_links.size(); }
                
                bool is_end_effector() const { assert (!_abody.expired()); return _child_links.empty(); }
 

                void set_child_link_link(RigidBodyPtr child, const Vector3& com_to_inner_joint_vec_link) { add_child_link_link(child, com_to_inner_joint_vec_link); }


                void set_child_link_global(RigidBodyPtr child, const Vector3& com_to_inner_joint_vec_global) { add_child_link_global(child, com_to_inner_joint_vec_global); }

                void clear_child_links() { _child_links.clear(); }

                unsigned get_index() { return _link_idx; }


                void set_index(unsigned index) { _link_idx = index; }

        private:        

                Real _mass;

                Real _mass_inv;

                Matrix3 _I;

                Matrix3 _Iinv;

                Vector3 _P;

                Vector3 _L;
         
                Vector3 _omega;

                Vector3 _x;
         
                Vector3 _xd;

                Quat _q;
         
                Quat _qd;

                Matrix4 _F;

                Vector3 _forces;

                Vector3 _torques;

                static const unsigned STATE_DIM = 13;

                void synchronize();

                unsigned _link_idx;

                SMatrix6 _Ii_iso;    

                SMatrixNPtr _Is0;
    
                SMatrixNPtr _Isi;    

                SMatrix6Ptr _X_im1_i;    

                SMatrix6Ptr _X_i_im1;    

                SMatrix6Ptr _X_0_i;    

                SMatrix6Ptr _X_i_0;    

                Vector3 _a;

                Vector3 _alpha; 

                std::vector<CollisionGeometryPtr> _geometries;

                bool _enabled;

                boost::weak_ptr<ArticulatedBody> _abody;

                Vector3 _d;

                boost::weak_ptr<RigidBody> _parent_link;

                boost::weak_ptr<Joint> _inner_joint;

                std::map<RigidBodyPtr, Vector3> _child_links; 
}; // end class

} // end namespace

#endif

---