00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef __CT_JOINT_H__
00022 #define __CT_JOINT_H__
00023
00024 #include "csphyzik/phyztype.h"
00025 #include "csphyzik/math3d.h"
00026 #include "csphyzik/ctvspat.h"
00027
00028 class ctArticulatedBody;
00029 class ctForce;
00030
00032 #define JOINT_STATESIZE 2
00033
00039 class ctJoint
00040 {
00041 public:
00043 static double joint_friction;
00044
00045 ctJoint ()
00046 : inboard_offset(0), outboard_offset(0), joint_axis(0)
00047 { q = qv = qa = 0; }
00048
00049 ctJoint ( ctArticulatedBody *in, ctVector3 &in_offset,
00050 ctArticulatedBody *out, ctVector3 &out_offset );
00051
00053 virtual void calc_vw( ctVector3 &v, ctVector3 &w ) = 0;
00059 virtual void calc_coriolus ( const ctVector3 &r, const ctVector3 &w_f,
00060 ctSpatialVector &c ) = 0;
00061
00062 virtual ctSpatialVector get_spatial_joint_axis() = 0;
00063
00068 virtual real get_actuator_magnitude( real external_f, real inertail_comp );
00069
00070 virtual int get_state_size(){ return JOINT_STATESIZE; }
00071 virtual int set_state( real *sa );
00072 virtual int get_state( const real *sa );
00073 virtual int set_delta_state( real *state_array );
00074
00076 void update_link_RF ();
00077
00079 ctVector3 get_r ();
00080
00081 ctArticulatedBody *inboard;
00086 ctVector3 inboard_offset;
00087 ctArticulatedBody *outboard;
00088
00093 ctVector3 outboard_offset;
00094
00096 ctVector3 joint_axis;
00098 real q, qv, qa;
00099
00100 protected:
00102
00103
00104 };
00105
00108 class ctPrismaticJoint : public ctJoint
00109 {
00110 public:
00111 virtual void calc_vw ( ctVector3 &v, ctVector3& )
00112 { v = v + joint_axis *qv; }
00113
00114 virtual void calc_coriolus ( const ctVector3 &r, const ctVector3 &w_f,
00115 ctSpatialVector &c );
00116
00117 virtual ctSpatialVector get_spatial_joint_axis ();
00118
00119 };
00120
00122 class ctRevoluteJoint : public ctJoint
00123 {
00124 public:
00125 ctRevoluteJoint ( ctArticulatedBody *in, ctVector3 &in_offset,
00126 ctArticulatedBody *out, ctVector3 &out_offset,
00127 ctVector3 &paxis );
00128
00129 virtual void calc_vw ( ctVector3 &v, ctVector3 &w )
00130 {
00131 w = w + joint_axis*qv;
00132 v = v + ( joint_axis % outboard_offset )*qv;
00133 }
00134
00135 virtual void calc_coriolus ( const ctVector3 &r, const ctVector3 &w_f,
00136 ctSpatialVector &c );
00137
00138 virtual ctSpatialVector get_spatial_joint_axis ();
00139 };
00140
00147 class ctConstrainedRevoluteJoint : public ctRevoluteJoint
00148 {
00149 public:
00150 ctConstrainedRevoluteJoint ( ctArticulatedBody *in, ctVector3 &in_offset,
00151 ctArticulatedBody *out, ctVector3 &out_offset,
00152 ctVector3 &paxis );
00153
00158 virtual real get_actuator_magnitude ( real external_f, real inertail_comp );
00159
00160 void set_constraint ( real pmax_angle, real pmin_angle )
00161 { max_angle = pmax_angle; min_angle = pmin_angle; }
00162
00163 void set_spring_constant( real pk )
00164 { k = pk; }
00165
00166 void set_damping_constant( real pk )
00167 { damping_k = pk; }
00168
00169 real max_angle;
00170 real min_angle;
00171
00173
00175 real k;
00177 real damping_k;
00178 };
00179
00180 #endif // __CT_JOINT_H__