00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
00036
00037
00038
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
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
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
00131
00132 ctMatrix3 Mwork;
00133 ctSpatialMatrix sMwork;
00134 ctSpatialVector ZaIac;
00135 };
00136
00137 #endif