Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00014 #include "psim.h"
00015 #include <assert.h>
00016
00017 int pSim::ConstraintForces(fVec& cf)
00018 {
00019 int i;
00020 for(i=0; i<n_joint; i++)
00021 {
00022 fVec& f_final = joint_info[i].pjoints[0]->f_final;
00023 int idx = 6*joint_info[i].pjoints[0]->joint->i_joint;
00024 cf(idx) = f_final(0);
00025 cf(idx+1) = f_final(1);
00026 cf(idx+2) = f_final(2);
00027 cf(idx+3) = f_final(3);
00028 cf(idx+4) = f_final(4);
00029 cf(idx+5) = f_final(5);
00030 }
00031 return 0;
00032 }
00033
00034 int pSim::TotalCost()
00035 {
00036 return subchains->total_cost();
00037 }
00038
00039 int pSubChain::total_cost()
00040 {
00041 if(!this) return 0;
00042
00043 int ret = 0;
00044 if(n_links > 1) ret = n_outer_joints*n_outer_joints;
00045 ret += children[0]->total_cost();
00046 if(children[0] != children[1])
00047 ret += children[1]->total_cost();
00048 return ret;
00049 }
00050
00051 int pSim::ScheduleDepth()
00052 {
00053 return subchains->schedule_depth();
00054 }
00055
00056 int pSubChain::schedule_depth()
00057 {
00058 if(!this || n_links == 1) return 0;
00059 int ret0 = 0, ret1 = 0;
00060 if(children[0]) ret0 = children[0]->schedule_depth();
00061 if(children[1]) ret1 = children[1]->schedule_depth();
00062 return (ret0 > ret1) ? (ret0+1) : (ret1+1);
00063 }
00064
00065 int pSim::NumLeaves()
00066 {
00067 return subchains->num_leaves();
00068 }
00069
00070 int pSubChain::num_leaves()
00071 {
00072 if(!this || n_links == 1) return 0;
00073 if(children[0] && children[1] &&
00074 children[0]->n_links == 1 && children[1]->n_links == 1)
00075 {
00076 return 1;
00077 }
00078 int ret = children[0]->num_leaves();
00079 if(children[0] != children[1])
00080 ret += children[1]->num_leaves();
00081 return ret;
00082 }
00083
00087 void pSim::Clear()
00088 {
00089 if(joint_info)
00090 {
00091 for(int i=0; i<n_joint; i++)
00092 {
00093 delete joint_info[i].pjoints[0];
00094 delete joint_info[i].pjoints[1];
00095 if(joint_info[i].plink) delete joint_info[i].plink;
00096 }
00097 delete[] joint_info;
00098 }
00099 if(subchains) delete subchains;
00100 joint_info = 0;
00101 subchains = 0;
00102 Chain::Clear();
00103 }
00104
00105 int pSim::clear_data()
00106 {
00107 if(joint_info)
00108 {
00109 for(int i=0; i<n_joint; i++)
00110 {
00111 delete joint_info[i].pjoints[0];
00112 delete joint_info[i].pjoints[1];
00113 if(joint_info[i].plink) delete joint_info[i].plink;
00114 }
00115 delete[] joint_info;
00116 }
00117 if(subchains) delete subchains;
00118 joint_info = 0;
00119 subchains = 0;
00120 return Chain::clear_data();
00121 }
00122
00123 int pSim::clear_contact()
00124 {
00125 int n_contacts = contact_vjoints.size();
00126 if(n_contacts == 0) return 0;
00127 int org_n_joint = n_joint;
00128 JointInfo* jinfo_save = joint_info;
00129 for(joint_list::iterator j=contact_vjoints.begin(); j!=contact_vjoints.end(); j++)
00130 {
00131 RemoveJoint(*j);
00132 delete *j;
00133 }
00134
00135 Chain::clear_data();
00136 #ifdef SEGA
00137 Chain::init();
00138 #else
00139 Chain::init(0);
00140 #endif
00141 joint_info = new JointInfo [n_joint];
00142 for(int i=0; i<org_n_joint; i++)
00143 {
00144 if(contact_vjoint_index(jinfo_save[i].pjoints[0]->joint) < 0)
00145 {
00146 Joint* j = jinfo_save[i].pjoints[0]->joint;
00147 joint_info[j->i_joint].pjoints[0] = jinfo_save[i].pjoints[0];
00148 joint_info[j->i_joint].pjoints[1] = jinfo_save[i].pjoints[1];
00149 joint_info[j->i_joint].plink = jinfo_save[i].plink;
00150 }
00151 else
00152 {
00153 delete jinfo_save[i].pjoints[0];
00154 delete jinfo_save[i].pjoints[1];
00155 if(jinfo_save[i].plink) delete jinfo_save[i].plink;
00156 }
00157 }
00158 if(jinfo_save) delete[] jinfo_save;
00159 contact_vjoints.clear();
00160 contact_relvels.clear();
00161 fric_coefs.clear();
00162 all_vjoints.clear();
00163 all_Jv.clear();
00164 all_Jr.clear();
00165 all_jdot_v.clear();
00166 all_jdot_r.clear();
00167 return 0;
00168 }
00169
00173 int pSim::init_contact()
00174 {
00175 int n_contacts = contact_vjoints.size();
00176
00177 int org_n_joint = n_joint;
00178
00179 Chain::clear_data();
00180 #ifdef SEGA
00181 Chain::init();
00182 #else
00183 Chain::init(0);
00184 #endif
00185
00186 JointInfo* jinfo_save = joint_info;
00187 joint_info = new JointInfo [n_joint];
00188 for(int i=0; i<org_n_joint; i++)
00189 {
00190 int new_index = jinfo_save[i].pjoints[0]->joint->i_joint;
00191 joint_info[new_index].pjoints[0] = jinfo_save[i].pjoints[0];
00192 joint_info[new_index].pjoints[1] = jinfo_save[i].pjoints[1];
00193 joint_info[new_index].plink = jinfo_save[i].plink;
00194 }
00195
00196 for(int i=0; i<n_contacts; i++)
00197 {
00198 Joint* j = contact_vjoints[i];
00199 assert(j->real);
00200 pJoint* pj0 = new pJoint(j, j->real);
00201 pJoint* pj1 = new pJoint(j, j->parent);
00202 pj0->plink = joint_info[j->real->i_joint].plink;
00203 if(j->parent)
00204 pj1->plink = joint_info[j->parent->i_joint].plink;
00205 pj1->pair = pj0;
00206 joint_info[j->i_joint].pjoints[0] = pj0;
00207 joint_info[j->i_joint].pjoints[1] = pj1;
00208 joint_info[j->i_joint].plink = 0;
00209 }
00210 if(jinfo_save) delete[] jinfo_save;
00211 subchains->init();
00212 return 0;
00213 }
00214
00215 #ifdef SEGA
00216 int pSim::init()
00217 #else
00218 int pSim::init(SceneGraph* sg)
00219 #endif
00220 {
00221 #ifdef SEGA
00222 int ret = Chain::init();
00223 #else
00224 int ret = Chain::init(sg);
00225 #endif
00226 if(ret) return ret;
00227 myinit();
00228 return 0;
00229 }
00230
00231 int pSim::myinit()
00232 {
00233 if(joint_info) delete[] joint_info;
00234 joint_info = 0;
00235 if(n_joint == 0) return 0;
00236 joint_info = new JointInfo[n_joint];
00237 setup_pjoint(root);
00238 setup_pjoint_virtual(root);
00239 calc_consts();
00240 return 0;
00241 }
00242
00243 void pSim::setup_pjoint(Joint* j)
00244 {
00245 if(!j) return;
00246 if(j->real) return;
00247 if(j->i_joint >= 0)
00248 {
00249 pJoint* pj0 = new pJoint(j, j);
00250 pJoint* pj1 = new pJoint(j, j->parent);
00251 pLink* pl = new pLink(j);
00252 pj0->plink = pl;
00253 if(j->parent)
00254 pj1->plink = joint_info[j->parent->i_joint].plink;
00255 pj0->pair = pj1;
00256 pj1->pair = pj0;
00257 joint_info[j->i_joint].pjoints[0] = pj0;
00258 joint_info[j->i_joint].pjoints[1] = pj1;
00259 joint_info[j->i_joint].plink = pl;
00260 }
00261 setup_pjoint(j->brother);
00262 setup_pjoint(j->child);
00263 }
00264
00265 void pSim::setup_pjoint_virtual(Joint* j)
00266 {
00267 if(!j) return;
00268 if(j->real)
00269 {
00270 pJoint* pj0 = new pJoint(j, j->real);
00271 pJoint* pj1 = new pJoint(j, j->parent);
00272
00273
00274 pj0->plink = joint_info[j->real->i_joint].plink;
00275 if(j->parent)
00276 pj1->plink = joint_info[j->parent->i_joint].plink;
00277 pj1->pair = pj0;
00278 joint_info[j->i_joint].pjoints[0] = pj0;
00279 joint_info[j->i_joint].pjoints[1] = pj1;
00280 joint_info[j->i_joint].plink = 0;
00281 }
00282 setup_pjoint_virtual(j->brother);
00283 setup_pjoint_virtual(j->child);
00284 }
00285
00286 void pSim::calc_consts()
00287 {
00288 int i;
00289 for(i=0; i<n_joint; i++)
00290 {
00291 if(joint_info[i].plink) joint_info[i].plink->calc_inertia();
00292 }
00293 }
00294
00295 void pLink::calc_inertia()
00296 {
00297 if(joint->n_root_dof == 0) return;
00298
00299 static fMat33 m11, m12, m22;
00300 double sx = joint->loc_com(0), sy = joint->loc_com(1), sz = joint->loc_com(2);
00301 fMat33 scross(0, -sz, sy,
00302 sz, 0, -sx,
00303 -sy, sx, 0);
00304 m11.identity();
00305 m11 *= joint->mass;
00306 m12.mul(joint->mass, scross);
00307 m22.mul(scross, scross);
00308 m22 *= -joint->mass;
00309 m22 += joint->inertia;
00310 if(joint->j_type == JROTATE)
00311 {
00312 static fVec3 n2J;
00313 static fMat33 n2Jmat;
00314 n2J.mul(joint->gear_ratio*joint->gear_ratio*joint->rotor_inertia, joint->axis);
00315 n2Jmat.mul(n2J, n2J);
00316 m22 += n2Jmat;
00317 }
00318 int i, j;
00319 for(i=0; i<3; i++)
00320 {
00321 for(j=0; j<3; j++)
00322 {
00323 M(i, j) = m11(i, j);
00324 M(i, j+3) = -m12(i, j);
00325 M(i+3, j) = m12(i, j);
00326 M(i+3, j+3) = m22(i, j);
00327 }
00328 }
00329 Minv.inv_posv(M);
00330
00331 }
00332
00333
00334
00335
00336 void pSim::DumpSchedule(ostream& ost)
00337 {
00338 subchains->dump(ost);
00339 }
00340
00341 void pSubChain::dump(ostream& ost)
00342 {
00343 if(!this) return;
00344 int i;
00345 ost << "-- pSubChain ->" << endl;
00346 if(last_pjoints[0]) ost << "\tlast: " << last_pjoints[0]->joint->name << endl;
00347 else ost << "\tsingle link" << endl;
00348 if(parent) ost << "\tparent: " << parent->last_pjoints[0]->joint->name << endl;
00349 ost << "\touter: " << n_outer_joints << endl;
00350 for(i=0; i<n_outer_joints; i++)
00351 outer_joints[i]->dump(ost);
00352 ost << "\tlinks: " << n_links << endl;
00353 for(i=0; i<n_links; i++)
00354 links[i]->dump(ost);
00355 #ifdef USE_MPI
00356 ost << "\trank: " << rank << endl;
00357 #endif
00358 ost << "<-" << endl;
00359 children[0]->dump(ost);
00360 if(children[1] != children[0]) children[1]->dump(ost);
00361 }
00362
00363 void pJoint::dump(ostream& ost)
00364 {
00365 ost << "\t\t" << joint->name << "->" << link_side->name << endl;
00366 }
00367
00368 void pLink::dump(ostream& ost)
00369 {
00370 ost << "\t\t" << joint->name << endl;
00371 }