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

00001 /****************************************************************************
00002  * Copyright 2005 Evan Drumwright
00003  * This library is distributed under the terms of the GNU General Public 
00004  * License (found in COPYING).
00005  ****************************************************************************/
00006 
00007 #ifndef _FS_AB_ALGORITHM_H
00008 #define _FS_AB_ALGORITHM_H
00009 
00010 #include <Physsim/SMatrix6.h>
00011 #include <Physsim/RCArticulatedBodyFwdDynAlgo.h>
00012 
00013 namespace Physsim {
00014 
00016 
00037 class FSABAlgorithm : public RCArticulatedBodyFwdDynAlgo 
00038 {
00039         public:
00040                 FSABAlgorithm() {}
00041                 virtual ~FSABAlgorithm() {}
00042                 virtual void load_state(BasePtr object, bimap<BasePtr, BasePtr>* correspondence);
00043                 virtual void save_state(BasePtr object, bimap<BasePtr, BasePtr>* correspondence) const;
00044                 virtual void clone(BasePtr& cloned, bimap<BasePtr, BasePtr>* obj_map = NULL) const;
00045                 virtual void calc_fwd_dyn(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00046                 virtual void apply_impulse(RCArticulatedBodyPtr body, const Vector3& j, const Vector3& k, const Vector3& contact_point, RigidBodyPtr link);
00047                 virtual void load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map);
00048                 virtual void save_to_xml(XMLTreePtr node, std::list<BaseConstPtr>& shared_objects) const;
00049 
00051                 ReferenceFrameType _rftype;
00052 
00054                 std::vector<SVector6> _v;
00055 
00057                 std::vector<SVector6> _a;
00058 
00060                 std::vector<SMatrix6> _Iiso;
00061 
00063                 std::vector<SMatrix6> _I;
00064 
00066                 std::vector<SVector6> _Z;
00067 
00069                 std::vector<SVector6> _c;
00070 
00072                 std::vector<SMatrixN> _Is;
00073 
00075                 std::vector<MatrixNN> _sIs;
00076 
00078                 std::vector<VectorN> _mu;
00079 
00080         private:
00081                 void set_spatial_iso_inertias(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00082                 void set_spatial_velocities(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00083                 void calc_spatial_accelerations(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00084                 void calc_spatial_ZAI(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00085                 void calc_spatial_coriolis_vectors(RCArticulatedBodyPtr body, ReferenceFrameType rftype);
00086 }; // end class
00087 } // end namespace
00088 
00089 #endif
00090