|
feathers.h00001 /* 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 |