00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "ik.h"
00015 #include <string>
00016
00017 IK::IK(): Chain()
00018 {
00019 n_assigned_constraints = 0;
00020 n_constraints = 0;
00021 n_total_const = 0;
00022 for(int m=0; m<N_PRIORITY_TYPES; m++)
00023 n_const[m] = 0;
00024 n_all_const = 0;
00025 max_condnum = MAX_CONDITION_NUMBER;
00026 assign_constraints(100);
00027 }
00028
00029 IK::~IK()
00030 {
00031 #ifdef MEASURE_TIME
00032 extern int n_update;
00033 extern double calc_jacobian_time;
00034 extern double solve_ik_time;
00035 extern double high_constraint_time;
00036 extern double low_constraint_time;
00037 cerr << "n_update = " << n_update << endl;
00038 cerr << "calc_jacobian = " << calc_jacobian_time << endl;
00039 cerr << "solve_ik = " << solve_ik_time << endl;
00040 cerr << "high_constraint = " << high_constraint_time << endl;
00041 cerr << "low_constraint = " << low_constraint_time << endl;
00042 #endif
00043
00044 for(int i=0; i<n_assigned_constraints; i++)
00045 {
00046 if(constraints[i]) delete constraints[i];
00047 }
00048 delete[] constraints;
00049 }
00050
00051 #ifdef SEGA
00052 int IK::init()
00053 #else
00054 int IK::init(SceneGraph* sg)
00055 #endif
00056 {
00057 #ifdef SEGA
00058 Chain::init();
00059 myinit();
00060 #else
00061 Chain::init(sg);
00062 myinit(sg);
00063 #endif
00064 return 0;
00065 }
00066
00067 #ifdef SEGA
00068 int IK::myinit()
00069 #else
00070 int IK::myinit(SceneGraph* sg)
00071 #endif
00072 {
00073 joint_weights.resize(n_dof);
00074 joint_weights = 1.0;
00075 #ifndef SEGA
00076
00077 if(sg)
00078 {
00079 CalcPosition();
00080 Joint* j;
00081 for(j=root->child; j; j=j->brother)
00082 load_markers(sg, j);
00083 }
00084 #endif
00085 return 0;
00086 }
00087
00088 #ifndef SEGA
00089 int IK::load_markers(SceneGraph* sg, Joint* rj)
00090 {
00091 char* marker_top_full_name;
00092 char* char_name = rj->CharName();
00093 if(char_name)
00094 {
00095 marker_top_full_name = new char [strlen(marker_top_name) + strlen(char_name) + 2];
00096 sprintf(marker_top_full_name, "%s%c%s", marker_top_name, charname_separator, char_name);
00097 }
00098 else
00099 {
00100 marker_top_full_name = new char [strlen(marker_top_name) + 1];
00101 strcpy(marker_top_full_name, marker_top_name);
00102 }
00103 TransformNode* marker_top = sg->findTransformNode(marker_top_full_name);
00104 if(marker_top)
00105 load_markers(marker_top);
00106 delete[] marker_top_full_name;
00107 return 0;
00108 }
00109
00110 int IK::load_markers(TransformNode* marker_top)
00111 {
00112
00113 Node* n;
00114 for(n=marker_top->getChildNodes(); n && !n->getName(); n=n->getChildNodes());
00115 if(!n) return 0;
00116 Node* tn;
00117 for(tn=n; tn; tn=tn->next())
00118 {
00119 if(tn->isTransformNode())
00120 add_marker((TransformNode*)tn);
00121 }
00122 return 0;
00123 }
00124
00125 IKHandle* IK::add_marker(TransformNode* tn)
00126 {
00127
00128 char* fullname = new char [strlen(tn->getName()) + 1];
00129 strcpy(fullname, tn->getName());
00130 char* ch_start = strrchr(fullname, charname_separator);
00131 char* body_name;
00132 if(ch_start)
00133 {
00134 body_name = new char [ch_start - fullname + 1];
00135 strncpy(body_name, fullname, ch_start-fullname);
00136 body_name[ch_start-fullname] = '\0';
00137 }
00138 else
00139 {
00140 body_name = new char [strlen(fullname) + 1];
00141 strcpy(body_name, fullname);
00142 }
00143 char* j_start = strrchr(body_name, joint_name_separator);
00144 if(!j_start)
00145 {
00146 delete[] body_name;
00147 delete[] fullname;
00148 return 0;
00149 }
00150 char* joint_name = new char [strlen(j_start)];
00151 strcpy(joint_name, j_start+1);
00152
00153 char* marker_name;
00154 if(ch_start)
00155 {
00156 marker_name = new char [j_start - body_name + strlen(ch_start) + 1];
00157 strncpy(marker_name, body_name, j_start-body_name);
00158 marker_name[j_start-body_name] = charname_separator;
00159 marker_name[j_start-body_name+1] = '\0';
00160 strcat(marker_name, ch_start+1);
00161 }
00162 else
00163 {
00164 marker_name = new char [j_start - body_name + 1];
00165 strncpy(marker_name, body_name, j_start-body_name);
00166 marker_name[j_start-body_name] = '\0';
00167 }
00168 Joint* joint;
00169 if(ch_start)
00170 joint = FindJoint(joint_name, ch_start+1);
00171 else
00172 joint = FindJoint(joint_name);
00173 if(!joint)
00174 {
00175 delete[] body_name;
00176 delete[] fullname;
00177 delete[] joint_name;
00178 delete[] marker_name;
00179 return 0;
00180 }
00181
00182 Joint* new_joint;
00183 if(ch_start)
00184 {
00185
00186
00187 char* marker_joint_name = new char [strlen(marker_name) + 1];
00188 sprintf(marker_joint_name, "%s", marker_name);
00189 new_joint = new Joint(marker_joint_name, JFIXED);
00190 delete[] marker_joint_name;
00191 }
00192 else
00193 {
00194 new_joint = new Joint(body_name, JFIXED);
00195 }
00196 if(AddJoint(new_joint, joint))
00197 {
00198 delete[] body_name;
00199 delete[] fullname;
00200 delete[] joint_name;
00201 delete[] marker_name;
00202 delete new_joint;
00203 return 0;
00204 }
00205
00206 float fpos[3], fatt[3][3], fscale[3];
00207 fVec3 abs_pos;
00208 fMat33 abs_att;
00209 get_abs_matrix(tn, fpos, fatt, fscale);
00210 abs_pos(0) = fpos[0];
00211 abs_pos(1) = fpos[1];
00212 abs_pos(2) = fpos[2];
00213 abs_att(0,0) = fatt[0][0];
00214 abs_att(0,1) = fatt[0][1];
00215 abs_att(0,2) = fatt[0][2];
00216 abs_att(1,0) = fatt[1][0];
00217 abs_att(1,1) = fatt[1][1];
00218 abs_att(1,2) = fatt[1][2];
00219 abs_att(2,0) = fatt[2][0];
00220 abs_att(2,1) = fatt[2][1];
00221 abs_att(2,2) = fatt[2][2];
00222 set_abs_position_orientation(new_joint, abs_pos, abs_att);
00223
00224 ConstIndex cindex[6] = {
00225 HAVE_CONSTRAINT,
00226 HAVE_CONSTRAINT,
00227 HAVE_CONSTRAINT,
00228 NO_CONSTRAINT,
00229 NO_CONSTRAINT,
00230 NO_CONSTRAINT,
00231 };
00232 IKHandle* h = new IKHandle(this, marker_name, new_joint, cindex, LOW_PRIORITY, 10.0);
00233 AddConstraint(h);
00234 cerr << "marker " << marker_name << " added to " << joint->name << " (abs_pos = " << abs_pos << ", rel_pos = " << new_joint->rel_pos << ")" << endl;
00235
00236 delete[] body_name;
00237 delete[] fullname;
00238 delete[] joint_name;
00239 delete[] marker_name;
00240 return h;
00241 }
00242
00243
00244 IKHandle* IK::AddMarker(const std::string& label, const std::string& linkname, const std::string& charname, const fVec3& rel_pos)
00245 {
00246 Joint* pjoint = FindJoint(linkname.c_str(), charname.c_str());
00247 if(!pjoint) return 0;
00248 std::string fullname = label + charname_separator + charname;
00249 Joint* new_joint = new Joint(fullname.c_str(), JFIXED);
00250 if(AddJoint(new_joint, pjoint))
00251 {
00252 delete new_joint;
00253 return 0;
00254 }
00255 new_joint->rel_pos.set(rel_pos);
00256
00257 ConstIndex cindex[6] = {
00258 HAVE_CONSTRAINT,
00259 HAVE_CONSTRAINT,
00260 HAVE_CONSTRAINT,
00261 NO_CONSTRAINT,
00262 NO_CONSTRAINT,
00263 NO_CONSTRAINT,
00264 };
00265 IKHandle* h = new IKHandle(this, fullname.c_str(), new_joint, cindex, LOW_PRIORITY, 10.0);
00266 AddConstraint(h);
00267 cerr << "marker " << fullname << " added " << new_joint->rel_pos << endl;
00268 return h;
00269 }
00270
00271 IKHandle* IK::AddMarker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos)
00272 {
00273 IKHandle* ret;
00274 in_create_chain = true;
00275 clear_data();
00276 ret = add_marker(marker_name, parent_joint, abs_pos);
00277 init(0);
00278 in_create_chain = false;
00279 return ret;
00280 }
00281
00282 IKHandle* IK::add_marker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos)
00283 {
00284 char* fullname = new char [strlen(marker_name) + strlen(parent_joint->name) + 2];
00285 sprintf(fullname, "%s%c%s", marker_name, joint_name_separator, parent_joint->name);
00286 TransformNode* tnode = new TransformNode;
00287 tnode->setName(fullname);
00288 tnode->setTranslation(abs_pos(0), abs_pos(1), abs_pos(2));
00289 IKHandle* ret = add_marker(tnode);
00290 delete[] fullname;
00291 delete tnode;
00292 return ret;
00293 }
00294
00295 int IK::EditMarker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos)
00296 {
00297 int ret;
00298 in_create_chain = true;
00299 clear_data();
00300 ret = edit_marker(marker, body_name, parent_joint, abs_pos);
00301 init(0);
00302 in_create_chain = false;
00303 return ret;
00304 }
00305
00306 int IK::edit_marker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos)
00307 {
00308
00309 char* ch_start = strrchr(parent_joint->name, charname_separator);
00310 if(marker->joint_name) delete[] marker->joint_name;
00311 marker->joint_name = 0;
00312 if(ch_start)
00313 {
00314 marker->joint_name = new char [strlen(body_name) + strlen(ch_start) + 1];
00315 sprintf(marker->joint_name, "%s%s", body_name, ch_start);
00316 cerr << "new name = " << marker->joint_name << endl;
00317 }
00318 else
00319 {
00320 marker->joint_name = new char [strlen(body_name) + 1];
00321 strcpy(marker->joint_name, body_name);
00322 }
00323 marker->joint->SetName(marker->joint_name);
00324
00325 if(marker->joint->parent == parent_joint) return 0;
00326
00327 cerr << "fixed to " << parent_joint->name << endl;
00328 RemoveJoint(marker->joint);
00329 AddJoint(marker->joint, parent_joint);
00330 fMat33 abs_att;
00331 abs_att.identity();
00332 set_abs_position_orientation(marker->joint, abs_pos, abs_att);
00333 return 0;
00334 }
00335
00336 int IK::SaveMarkers(const char* _fname)
00337 {
00338 CalcPosition();
00339 SceneGraph* sg = new SceneGraph;
00340 int i;
00341 TransformNode* top_tnode = new TransformNode;
00342 top_tnode->setName(marker_top_name);
00343 sg->addNode(top_tnode);
00344 for(i=0; i<n_constraints; i++)
00345 {
00346 if(constraints[i]->GetType() == HANDLE_CONSTRAINT)
00347 save_marker(top_tnode, (IKHandle*)constraints[i]);
00348 }
00349 char* fname = new char [strlen(_fname) + 1];
00350 strcpy(fname, _fname);
00351 sg->save(fname);
00352 delete sg;
00353 delete[] fname;
00354 return 0;
00355 }
00356
00357 int IK::save_marker(TransformNode* top_tnode, IKHandle* h)
00358 {
00359 char* char_name = h->joint->CharName();
00360 char* t_name;
00361 if(char_name)
00362 {
00363 int marker_name_length = strlen(h->joint_name) - strlen(char_name) - 1;
00364 int parent_name_length = strlen(h->joint->parent->name) - strlen(char_name) - 1;
00365 char* marker_name = new char [marker_name_length + 1];
00366 char* parent_name = new char [parent_name_length + 1];
00367 t_name = new char [marker_name_length + parent_name_length + 2];
00368 strncpy(marker_name, h->joint_name, marker_name_length);
00369 marker_name[marker_name_length] = '\0';
00370 strncpy(parent_name, h->joint->parent->name, parent_name_length);
00371 parent_name[parent_name_length] = '\0';
00372 sprintf(t_name, "%s%c%s", marker_name, joint_name_separator, parent_name);
00373 delete[] marker_name;
00374 delete[] parent_name;
00375 }
00376 else
00377 {
00378 t_name = new char [strlen(h->joint_name) + strlen(h->joint->parent->name) + 2];
00379 sprintf(t_name, "%s%c%s", h->joint_name, joint_name_separator, h->joint->parent->name);
00380 }
00381 CalcPosition();
00382 TransformNode* tnode = new TransformNode();
00383 tnode->setName(t_name);
00384 tnode->setTranslation(h->joint->abs_pos(0), h->joint->abs_pos(1), h->joint->abs_pos(2));
00385 top_tnode->addChildNode(tnode);
00386 delete[] t_name;
00387 return 0;
00388 }
00389
00390 #endif
00391
00392 int IK::NumConstraints(ConstType t)
00393 {
00394 int count = 0;
00395 for(int i=0; i<n_constraints; i++)
00396 {
00397 if(constraints[i]->GetType() == t) count++;
00398 }
00399 return count;
00400 }
00401
00402 int IK::assign_constraints(int _n)
00403 {
00404 cerr << "assign_constraints(" << _n << ")" << endl;
00405 int i;
00406 IKConstraint** save = 0;
00407 if(constraints && n_assigned_constraints > 0)
00408 {
00409 save = new IKConstraint* [n_assigned_constraints];
00410 for(i=0; i<n_assigned_constraints; i++)
00411 save[i] = constraints[i];
00412 delete[] constraints;
00413 }
00414 constraints = 0;
00415 if(_n > 0)
00416 {
00417 constraints = new IKConstraint* [_n];
00418 for(i=0; i<_n; i++) constraints[i] = 0;
00419 for(i=0; i<n_assigned_constraints; i++)
00420 constraints[i] = save[i];
00421 }
00422 n_assigned_constraints = _n;
00423 if(save) delete[] save;
00424 cerr << "<-" << endl;
00425 return 0;
00426 }
00427
00428 int IK::AddConstraint(IKConstraint* _constraint)
00429 {
00430
00431 if(n_constraints >= n_assigned_constraints)
00432 assign_constraints(n_assigned_constraints*2);
00433 int id = n_total_const++;
00434 constraints[n_constraints++] = _constraint;
00435 _constraint->id = id;
00436 return id;
00437 }
00438
00439 int IK::RemoveConstraint(int _id)
00440 {
00441 int index = ConstraintIndex(_id);
00442 if(index < 0) return -1;
00443 if(constraints[index]) delete constraints[index];
00444 constraints[index] = 0;
00445 int i;
00446 for(i=index+1; i<n_constraints; i++)
00447 {
00448 constraints[i-1] = constraints[i];
00449 }
00450 constraints[--n_constraints] = 0;
00451 return 0;
00452 }
00453
00454 int IK::RemoveAllConstraints()
00455 {
00456 for(int i=0; i<n_constraints; i++)
00457 {
00458 if(constraints[i]) delete constraints[i];
00459 constraints[i] = 0;
00460 }
00461 n_constraints = 0;
00462 return 0;
00463 }
00464
00465 IKConstraint* IK::FindConstraint(ConstType _type, const char* jname, const char* charname)
00466 {
00467 static char fullname[256];
00468 if(charname) {
00469 sprintf(fullname, "%s%c%s", jname, charname_separator, charname);
00470 }
00471 else {
00472 strcpy(fullname, jname);
00473 }
00474 for(int i=0; i<n_constraints; i++)
00475 {
00476 if(constraints[i]->GetType() == _type &&
00477 !strcmp(fullname, constraints[i]->joint_name))
00478 return constraints[i];
00479 }
00480 return 0;
00481 }
00482
00483 IKConstraint* IK::FindConstraint(int _id)
00484 {
00485 for(int i=0; i<n_constraints; i++)
00486 {
00487 if(constraints[i]->id == _id)
00488 return constraints[i];
00489 }
00490 return 0;
00491 }
00492
00493 int IK::ConstraintID(ConstType _type, const char* jname)
00494 {
00495 for(int i=0; i<n_constraints; i++)
00496 {
00497 if(constraints[i]->GetType() == _type &&
00498 !strcmp(jname, constraints[i]->joint_name))
00499 return constraints[i]->id;
00500 }
00501 return -1;
00502 }
00503
00504 int IK::ConstraintID(int _index)
00505 {
00506 if(_index < 0 || _index >= n_constraints)
00507 return -1;
00508 return constraints[_index]->id;
00509 }
00510
00511 int IK::ConstraintIndex(ConstType _type, const char* jname)
00512 {
00513 for(int i=0; i<n_constraints; i++)
00514 {
00515 if(constraints[i]->GetType() == _type &&
00516 !strcmp(jname, constraints[i]->joint_name))
00517 return i;
00518 }
00519 return -1;
00520 }
00521
00522 int IK::ConstraintIndex(int _id)
00523 {
00524 for(int i=0; i<n_constraints; i++)
00525 {
00526 if(constraints[i]->id == _id)
00527 return i;
00528 }
00529 return -1;
00530 }
00531
00532 int IK::ResetAllConstraints()
00533 {
00534 int i;
00535 for(i=0; i<n_constraints; i++)
00536 constraints[i]->Reset();
00537 return 0;
00538 }
00539
00540 int IK::ResetConstraints(ConstType t)
00541 {
00542 int i;
00543 for(i=0; i<n_constraints; i++)
00544 {
00545 if(constraints[i]->GetType() == t)
00546 constraints[i]->Reset();
00547 }
00548 return 0;
00549 }
00550
00551 int IK::SetJointWeight(const char* jname, double _weight)
00552 {
00553 double weight = _weight;
00554 if(weight < MIN_JOINT_WEIGHT) weight = MIN_JOINT_WEIGHT;
00555 Joint* jnt = FindJoint(jname);
00556 if(!jnt) return -1;
00557 int i;
00558 for(i=0; i<jnt->n_dof; i++)
00559 {
00560 joint_weights(i+jnt->i_dof) = weight;
00561 }
00562 return 0;
00563 }
00564
00565 int IK::SetJointWeight(const char* jname, const fVec& _weight)
00566 {
00567 Joint* jnt = FindJoint(jname);
00568 if(!jnt) return -1;
00569 int i;
00570 for(i=0; i<jnt->n_dof && i<_weight.size(); i++)
00571 {
00572 if(_weight(i) >= MIN_JOINT_WEIGHT)
00573 joint_weights(i+jnt->i_dof) = _weight(i);
00574 }
00575 return 0;
00576 }
00577
00578 int IK::SetDesireGain(const char* jname, double _gain)
00579 {
00580 if(_gain < 0.0) return -1;
00581 IKDesire* des = (IKDesire*)FindConstraint(DESIRE_CONSTRAINT, jname);
00582 if(!des) return -1;
00583 des->gain = _gain;
00584 return 0;
00585 }
00586
00587 int IK::EnableDesire(const char* jname)
00588 {
00589 IKDesire* des = (IKDesire*)FindConstraint(DESIRE_CONSTRAINT, jname);
00590 if(!des) return -1;
00591 des->Enable();
00592 cerr << "desire constraint at " << jname << " enabled" << endl;
00593 return 0;
00594 }
00595
00596 int IK::DisableDesire(const char* jname)
00597 {
00598 IKDesire* des = (IKDesire*)FindConstraint(DESIRE_CONSTRAINT, jname);
00599 if(!des) return -1;
00600 des->Disable();
00601 cerr << "desire constraint at " << jname << " disabled" << endl;
00602 return 0;
00603 }
00604
00605 void IK::SetCharacterScale(double _scale, const char* charname)
00606 {
00607 SetConstraintScale(_scale, charname);
00608
00609 joint_weights.resize(n_dof);
00610 joint_weights = 1.0;
00611 set_character_scale(root, _scale, charname);
00612 }
00613
00614 void IK::SetConstraintScale(double _scale, const char* charname)
00615 {
00616 int i;
00617 for(i=0; i<n_constraints; i++)
00618 {
00619 constraints[i]->SetCharacterScale(_scale, charname);
00620 }
00621 }
00622
00623 void IK::set_character_scale(Joint* jnt, double _scale, const char* charname)
00624 {
00625 if(!jnt) return;
00626 if((!charname || strstr(jnt->name, charname)) && jnt->n_dof > 0)
00627 {
00628
00629 switch(jnt->j_type)
00630 {
00631 case JROTATE:
00632 joint_weights(jnt->i_dof) = _scale;
00633 break;
00634 case JSPHERE:
00635 joint_weights(jnt->i_dof) = _scale;
00636 joint_weights(jnt->i_dof+1) = _scale;
00637 joint_weights(jnt->i_dof+2) = _scale;
00638 break;
00639 case JFREE:
00640 joint_weights(jnt->i_dof+3) = _scale;
00641 joint_weights(jnt->i_dof+4) = _scale;
00642 joint_weights(jnt->i_dof+5) = _scale;
00643 break;
00644 default:
00645 break;
00646 }
00647 }
00648 set_character_scale(jnt->brother, _scale, charname);
00649 set_character_scale(jnt->child, _scale, charname);
00650 }
00651
00652 void IKHandle::SetCharacterScale(double _scale, const char* charname)
00653 {
00654 if(!joint) return;
00655 if(n_const == 0) return;
00656 if(charname && !strstr(joint->name, charname)) return;
00657 int i;
00658 weight.resize(n_const);
00659 weight = 1.0;
00660 int count = 0;
00661
00662 double s1 = _scale;
00663 for(i=0; i<6; i++)
00664 {
00665 if(const_index[i] == IK::HAVE_CONSTRAINT)
00666 {
00667
00668 if(i >= 3) weight(count) = s1;
00669 count++;
00670 }
00671 }
00672 }
00673
00674 void IKDesire::SetCharacterScale(double _scale, const char* charname)
00675 {
00676 if(!joint) return;
00677 if(n_const == 0) return;
00678 if(charname && !strstr(joint->name, charname)) return;
00679 weight.resize(n_const);
00680 if(_scale < 1.0)
00681 weight = _scale;
00682 else
00683 weight = 1.0;
00684 #if 0
00685
00686 double s1 = _scale;
00687
00688 switch(joint->j_type)
00689 {
00690 case JROTATE:
00691 weight(0) = s1;
00692 break;
00693 case JSPHERE:
00694 weight(0) = s1;
00695 weight(1) = s1;
00696 weight(2) = s1;
00697 break;
00698 case JFREE:
00699 weight(3) = s1;
00700 weight(4) = s1;
00701 weight(5) = s1;
00702 break;
00703 default:
00704 break;
00705 }
00706 #endif
00707 }