Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "chain.h"
00015
00016 void Chain::CalcPosition()
00017 {
00018 root->calc_position();
00019 }
00020
00021 void Joint::calc_position()
00022 {
00023 if(parent)
00024 {
00025 #if 0
00026
00027 abs_pos = parent->abs_pos + parent->abs_att * rel_pos;
00028 abs_att = parent->abs_att * rel_att;
00029 #else
00030
00031 abs_pos.mul(parent->abs_att, rel_pos);
00032 abs_pos += parent->abs_pos;
00033 abs_att.mul(parent->abs_att, rel_att);
00034 #endif
00035
00036 }
00037 child->calc_position();
00038 brother->calc_position();
00039 }
00040
00041 void Chain::CalcVelocity()
00042 {
00043 root->calc_velocity();
00044 }
00045
00046 void Joint::calc_velocity()
00047 {
00048 if(parent)
00049 {
00050 static fMat33 t_rel_att;
00051 static fVec3 v1, v2;
00052 t_rel_att.tran(rel_att);
00053
00054 v1.cross(parent->loc_ang_vel, rel_pos);
00055 v2.mul(t_rel_att, parent->loc_lin_vel);
00056 loc_lin_vel.mul(t_rel_att, v1);
00057 loc_lin_vel += v2;
00058 loc_lin_vel += rel_lin_vel;
00059
00060 loc_ang_vel.mul(t_rel_att, parent->loc_ang_vel);
00061 loc_ang_vel += rel_ang_vel;
00062
00063 v1.cross(loc_ang_vel, loc_com);
00064 loc_com_vel.add(loc_lin_vel, v1);
00065 }
00066 else
00067 {
00068 loc_lin_vel.zero();
00069 loc_ang_vel.zero();
00070 }
00071 child->calc_velocity();
00072 brother->calc_velocity();
00073 }
00074
00075 void Chain::CalcAcceleration()
00076 {
00077 root->calc_acceleration();
00078 }
00079
00080 void Joint::calc_acceleration()
00081 {
00082 if(parent)
00083 {
00084 static fMat33 t_rel_att;
00085 static fVec3 v1, v2, v3, v4;
00086 t_rel_att.tran(rel_att);
00087
00088 v1.mul(t_rel_att, parent->loc_ang_vel);
00089
00090 v1 *= 2.0;
00091 v2.cross(v1, rel_lin_vel);
00092 loc_lin_acc.add(rel_lin_acc, v2);
00093 v2.cross(parent->loc_ang_acc, rel_pos);
00094 v1.add(parent->loc_lin_acc, v2);
00095 v2.cross(parent->loc_ang_vel, rel_pos);
00096 v3.cross(parent->loc_ang_vel, v2);
00097 v1 += v3;
00098 v4.mul(t_rel_att, v1);
00099 loc_lin_acc += v4;
00100
00101
00102 v1.cross(loc_ang_vel, rel_ang_vel);
00103 loc_ang_acc.add(rel_ang_acc, v1);
00104 v1.mul(t_rel_att, parent->loc_ang_acc);
00105 loc_ang_acc += v1;
00106
00107 v1.cross(loc_ang_acc, loc_com);
00108 loc_com_acc.add(loc_lin_acc, v1);
00109 v1.cross(loc_ang_vel, loc_com);
00110 v2.cross(loc_ang_vel, v1);
00111 loc_com_acc += v2;
00112
00113
00114
00115
00116
00117 }
00118 else
00119 {
00120
00121
00122 loc_ang_acc.zero();
00123 loc_com_acc.zero();
00124 }
00125 child->calc_acceleration();
00126 brother->calc_acceleration();
00127 }
00128
00129 double Chain::TotalCOM(fVec3& com, const char* chname)
00130 {
00131 com.zero();
00132 double m = root->total_com(com, chname);
00133 com /= m;
00134 return m;
00135 }
00136
00137 double Joint::total_com(fVec3& com, const char* chname)
00138 {
00139 int is_target = false;
00140 if(!chname)
00141 {
00142 is_target = true;
00143 }
00144 else
00145 {
00146 char* my_chname = CharName();
00147 if(my_chname && !strcmp(my_chname, chname))
00148 {
00149 is_target = true;
00150 }
00151 }
00152 fVec3 b_com, c_com;
00153 b_com.zero();
00154 c_com.zero();
00155 double ret = brother->total_com(b_com, chname) + child->total_com(c_com, chname);
00156
00157 com.add(b_com, c_com);
00158 if(is_target)
00159 {
00160 static fVec3 abs_com_pos, my_com;
00161 ret += mass;
00162 abs_com_pos.mul(abs_att, loc_com);
00163 abs_com_pos += abs_pos;
00164 my_com.mul(abs_com_pos, mass);
00165 com += my_com;
00166 }
00167 return ret;
00168 }
00169