Google

Main Page   Class Hierarchy   Compound List   File List   Compound Members  

feathers.h

00001 /*
00002     Dynamics/Kinematics modeling and simulation library.
00003     Copyright (C) 1999 by Michael Alexander Ewert
00004 
00005     This library is free software; you can redistribute it and/or
00006     modify it under the terms of the GNU Library General Public
00007     License as published by the Free Software Foundation; either
00008     version 2 of the License, or (at your option) any later version.
00009 
00010     This library is distributed in the hope that it will be useful,
00011     but WITHOUT ANY WARRANTY; without even the implied warranty of
00012     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013     Library General Public License for more details.
00014 
00015     You should have received a copy of the GNU Library General Public
00016     License along with this library; if not, write to the Free
00017     Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00018 
00019 */
00020 
00021 #ifndef CT_FEATHERSTONE_ALGORITHM
00022 #define CT_FEATHERSTONE_ALGORITHM
00023 
00027 
00028 #include "csphyzik/solver.h"
00029 #include "csphyzik/math3d.h"
00030 #include "csphyzik/ctvspat.h"
00031 #include "csphyzik/ctmspat.h"
00032 
00033 class ctArticulatedBody;
00034 
00035 // determine the motion of an articulated body acting under the influence of
00036 // externally ( e.g. gravity ) applied forces and internally ( e.g. robot motor )
00037 // applied forces
00038 // Only works for tree-like topologies
00039 class ctFeatherstoneAlgorithm : public ctArticulatedSolver
00040 {
00041 public:
00042   ctFeatherstoneAlgorithm ( ctArticulatedBody &pab )
00043     : ab( pab ){ sIsQsZIc_computed = false; };
00044 
00046   virtual void solve ( real t );
00047 
00048   //    void init();
00049 
00054   ctVector3 get_linear_a()
00055   { ctVector3 aret( a[3], a[4], a[5] ); return aret; }
00056 
00057   ctVector3 get_angular_a()
00058   { ctVector3 aret( a[0], a[1], a[2] ); return aret; }
00059 
00065   void apply_impulse( ctVector3 impulse_point, ctVector3 impulse_vector );
00066 
00071   void get_impulse_m_and_I_inv ( real *pm, ctMatrix3 *pI_inv,
00072     const ctVector3 &impulse_point, const ctVector3 &unit_length_impulse_vector );
00073 
00074 protected:
00075 
00076   void fsolve_grounded ( real t );
00077   void fsolve_floating ( real t );
00078 
00079   void init_link ();
00080 
00081   void compute_Ia_Za ();
00082 
00083   void compute_joint_a ();
00084 
00085   void impulse_to_v ();
00086 
00087   void test_impulse_response ();
00088 
00089   void propagate_impulse ();
00090 
00091   void zero_Ja_help ();
00092 
00093   void zero_Ja ();
00094 
00096   ctArticulatedBody &ab;
00097 
00098   // work variables
00099 
00100   ctSpatialVector a;
00101 
00103   ctSpatialMatrix Ia;
00104 
00111   ctSpatialVector Za;
00112 
00114   ctSpatialVector c;
00115 
00117   ctSpatialMatrix gXf;
00118 
00119 
00121   real sIs;
00122   real QsZIc;
00123   bool sIsQsZIc_computed;
00124 
00126   ctSpatialVector Ja;
00127   ctSpatialVector dv;
00128 
00129 private:
00130   // some work variables that I don't want piled on the stack during recursion
00131   // should convert to iteration actually for better performance
00132   ctMatrix3 Mwork;
00133   ctSpatialMatrix sMwork;
00134   ctSpatialVector ZaIac;
00135 };
00136 
00137 #endif

Generated for Crystal Space by doxygen 1.2.5 written by Dimitri van Heesch, ©1997-2000