Physsim: /home/drumwrig/Physsim/include/Physsim/CRBAlgorithm.h Source File

00001 /****************************************************************************
00002  * Copyright 2006 Evan Drumwright
00003  * This library is distributed under the terms of the GNU General Public 
00004  * License (found in COPYING).
00005  ****************************************************************************/
00006 
00007 #ifndef _CRB_ALGORITHM_H
00008 #define _CRB_ALGORITHM_H
00009 
00010 #include <Physsim/MatrixNN.h>
00011 #include <Physsim/SMatrix6.h>
00012 #include <Physsim/RCArticulatedBodyFwdDynAlgo.h>
00013 
00014 namespace Physsim {
00015 
00017 class CRBAlgorithm : public RCArticulatedBodyFwdDynAlgo
00018 {
00019         public:
00020                 CRBAlgorithm() {}
00021                 virtual ~CRBAlgorithm() {}
00022                 virtual void clone(BasePtr& cloned, bimap<BasePtr, BasePtr>* obj_map = NULL) const;
00023                 virtual void load_state(BaseConstPtr object, bimap<BasePtr, BasePtr>* correspondence = NULL);
00024                 virtual void save_state(BasePtr object, bimap<BasePtr, BasePtr>* correspondence = NULL) const;
00025                 virtual void calc_fwd_dyn(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00026                 virtual void apply_impulse(RCArticulatedBodyPtr body, const Vector3& j, const Vector3& k, const Vector3& contact_point, RigidBodyPtr link);
00027                 virtual void load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map);
00028                 virtual void save_to_xml(XMLTreePtr node, std::list<BaseConstPtr>& shared_objects) const;
00029 
00031                 std::vector<SVector6> _v;
00032 
00034                 SVector6 _a0;
00035 
00037                 VectorN _qdd;
00038 
00040                 std::vector<SMatrix6> _Iiso;
00041 
00043                 SMatrix6 _Ic0;
00044 
00046                 SVector6 _f0;
00047 
00049                 SMatrixN _K;
00050 
00052                 MatrixNN _inv_aug;
00053 
00055                 MatrixNN _H;
00056 
00058                 MatrixNN _inv_H;
00059 
00061                 VectorN _Q;
00062 
00064 
00068                 VectorN _C;
00069                         
00070         private:
00071                 void apply_impulse_fixed_base(RCArticulatedBodyPtr body, const Vector3& j, const Vector3& k, const Vector3& point, RigidBodyPtr link);
00072                 void apply_impulse_floating_base(RCArticulatedBodyPtr body, const Vector3& j, const Vector3& k, const Vector3& point, RigidBodyPtr link);
00073                 void set_spatial_iso_inertias(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00074                 void set_spatial_velocities(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00075                 void calc_fwd_dyn_fixed_base(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00076                 void calc_fwd_dyn_floating_base(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00077                 SVector6 calc_C(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00078                 void update_link_accelerations(RCArticulatedBodyPtr body, ReferenceFrameType rftype) const;
00079 };
00080 }
00081 
00082 #endif