00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "chain.h"
00015
00016
00017
00018
00019 int Chain::BeginCreateChain(int append)
00020 {
00021 if(in_create_chain)
00022 {
00023 cerr << "Chain::BeginCreateChain - called after BeginCreateChain()" << endl;
00024 return -1;
00025 }
00026 if(!append) Clear();
00027 in_create_chain = true;
00028 clear_data();
00029 return 0;
00030 }
00031
00032 int Chain::RemoveJoint(Joint* j)
00033 {
00034 if(!in_create_chain)
00035 {
00036 cerr << "Chain::RemoveJoint - attempted to edit the chain before calling BeginCreateChain" << endl;
00037 return -1;
00038 }
00039 if(!j->parent) return -1;
00040 return j->parent->remove_child(j);
00041 }
00042
00043 int Joint::remove_child(Joint* j)
00044 {
00045 Joint* prev = 0;
00046 j->parent = 0;
00047 if(child == j)
00048 {
00049 child = j->brother;
00050 j->brother = 0;
00051 return 0;
00052 }
00053 for(prev=child; prev && prev->brother!=j; prev=prev->brother);
00054 if(!prev) return -1;
00055 prev->brother = j->brother;
00056 j->brother = 0;
00057 return 0;
00058 }
00059
00060 Joint* Chain::AddRoot(const char* name, const fVec3& grav)
00061 {
00062 if(!in_create_chain)
00063 {
00064 cerr << "Chain::AddRoot - attempted to add a root before calling BeginCreateChain" << endl;
00065 return NULL;
00066 }
00067 if(root)
00068 {
00069 cerr << "Chain::AddRoot - root is already set" << endl;
00070 return NULL;
00071 }
00072 Joint* r;
00073 if(name)
00074 {
00075 r = new Joint(name, JFIXED);
00076 }
00077 else
00078 {
00079 r = new Joint("space", JFIXED);
00080 }
00081 r->loc_lin_acc.set(grav);
00082 r->chain = this;
00083 root = r;
00084 return r;
00085 }
00086
00087 int Chain::AddRoot(Joint* r)
00088 {
00089 if(!in_create_chain)
00090 {
00091 cerr << "Chain::AddRoot - attempted to add a root before calling BeginCreateChain" << endl;
00092 return -1;
00093 }
00094 if(root)
00095 {
00096 cerr << "Chain::AddRoot - root is already set" << endl;
00097 return -1;
00098 }
00099 r->chain = this;
00100 r->j_type = JFIXED;
00101 root = r;
00102 return 0;
00103 }
00104
00105 int Chain::AddJoint(Joint* target, const char* parent_name, const char* charname)
00106 {
00107 Joint* p = FindJoint(parent_name, charname);
00108 if(!strcmp(parent_name, root->name)) p = root;
00109 if(!p)
00110 {
00111 cerr << "Chain::AddJoint - parent " << parent_name << " not found for " << target->name << endl;
00112 return -1;
00113 }
00114 return AddJoint(target, p);
00115 }
00116
00117 int Chain::AddJoint(Joint* target, Joint* p)
00118 {
00119 if(!in_create_chain)
00120 {
00121 cerr << "Chain::AddJoint - attempted to add a joint before calling BeginCreateChain" << endl;
00122 return -1;
00123 }
00124 if(!root)
00125 {
00126 cerr << "Chain::AddJoint - root is not set" << endl;
00127 return -1;
00128 }
00129 if(!p)
00130 {
00131 cerr << "Chain::AddJoint - parent is NULL" << endl;
00132 return -1;
00133 }
00134 if(!p->chain)
00135 {
00136 cerr << "Joint::AddJoint - parent joint is not added yet" << endl;
00137 return -1;
00138 }
00139 if(FindJoint(target->name))
00140 {
00141 cerr << "Joint::AddJoint - joint " << target->name << " already exists" << endl;
00142 return -1;
00143 }
00144 if(!target) return 0;
00145 target->chain = this;
00146 if(p)
00147 {
00148 p->add_child(target);
00149 }
00150 else
00151 {
00152 root = target;
00153 }
00154 return 0;
00155 }
00156
00157 void Joint::add_child(Joint* c)
00158 {
00159 c->parent = this;
00160 c->brother = child;
00161 child = c;
00162 }
00163
00164 Joint* Chain::AddJoint(JointData* joint_data, const char* charname)
00165 {
00166 Joint* j = new Joint(joint_data, charname);
00167 if(AddJoint(j, joint_data->parent_name, charname)) return 0;
00168 return j;
00169 }
00170
00171 #if 0
00172 int Chain::CreateSerial(int num_joint, const JointData& joint_data,
00173 const char* charname, Joint* parent_joint)
00174 {
00175 if(!joint_data.name) return -1;
00176 if(!root) AddRoot();
00177 Joint* last_joint = root;
00178 if(parent_joint) last_joint = parent_joint;
00179 int i;
00180 for(i=0; i<num_joint; i++)
00181 {
00182 JointData* jdata = new JointData(joint_data);
00183 jdata->name = new char [strlen(joint_data.name) + 5];
00184 sprintf(jdata->name, "%s%04d", joint_data.name, i+1);
00185 jdata->parent_name = new char [strlen(last_joint->basename) + 1];
00186 strcpy(jdata->parent_name, last_joint->basename);
00187 Joint* new_joint = AddJoint(jdata, charname);
00188 cerr << new_joint->name << " added to " << last_joint->basename << endl;
00189 last_joint = new_joint;
00190 delete jdata;
00191 }
00192 return 0;
00193 }
00194
00195 int Chain::CreateParallel(int num_char, const char* prmname, const char* charname_base, const fVec3& init_pos, const fMat33& init_att, const fVec3& pos_offset, const fMat33& att_offset, int init_num)
00196 {
00197 int i;
00198 fVec3 cur_pos_offset(init_pos);
00199 fMat33 cur_att_offset(init_att);
00200 for(i=0; i<num_char; i++)
00201 {
00202 char* charname = new char [strlen(charname_base) + 5];
00203 sprintf(charname, "%s%04d", charname_base, i+init_num);
00204 Load(prmname, charname);
00205 Joint* char_root = FindCharacterRoot(charname);
00206 fVec3 tmp, tmpp;
00207 fMat33 tmpr;
00208 tmp.mul(cur_att_offset, pos_offset);
00209 cur_pos_offset += tmp;
00210 tmpr.mul(cur_att_offset, att_offset);
00211 cur_att_offset.set(tmpr);
00212 tmpp.add(char_root->rel_pos, cur_pos_offset);
00213 tmpr.mul(char_root->rel_att, cur_att_offset);
00214 char_root->SetJointValue(tmpp, tmpr);
00215 cerr << charname << ": " << char_root->rel_pos << endl << char_root->rel_att << endl;
00216 delete[] charname;
00217 }
00218 return 0;
00219 }
00220 #endif
00221
00222 #ifdef SEGA
00223 int Chain::EndCreateChain()
00224 #else
00225 int Chain::EndCreateChain(SceneGraph* sg)
00226 #endif
00227 {
00228 if(!in_create_chain)
00229 {
00230 cerr << "Chain::EndCreateChain - called before BeginCreateChain()" << endl;
00231 return -1;
00232 }
00233 #ifdef SEGA
00234 init();
00235 #else
00236 init(sg);
00237 #endif
00238 in_create_chain = false;
00239 return 0;
00240 }
00241
00242 #ifndef SEGA
00243 void Chain::set_relative_positions(SceneGraph* sg)
00244 {
00245 root->abs_pos.zero();
00246 root->abs_att.identity();
00247 calc_abs_positions(root, sg);
00248 calc_rel_positions(root, sg);
00249 }
00250
00251 void Chain::calc_abs_positions(Joint* cur, SceneGraph* sg)
00252 {
00253 if(!cur) return;
00254 TransformNode* tnode = sg->findTransformNode(cur->name);
00255
00256 if(cur->parent && tnode)
00257 {
00258 float abs_pos[3], abs_att[3][3], abs_scale[3];
00259 get_abs_matrix(tnode, abs_pos, abs_att, abs_scale);
00260 cur->abs_pos(0) = abs_pos[0];
00261 cur->abs_pos(1) = abs_pos[1];
00262 cur->abs_pos(2) = abs_pos[2];
00263 cur->abs_att(0,0) = abs_att[0][0];
00264 cur->abs_att(0,1) = abs_att[0][1];
00265 cur->abs_att(0,2) = abs_att[0][2];
00266 cur->abs_att(1,0) = abs_att[1][0];
00267 cur->abs_att(1,1) = abs_att[1][1];
00268 cur->abs_att(1,2) = abs_att[1][2];
00269 cur->abs_att(2,0) = abs_att[2][0];
00270 cur->abs_att(2,1) = abs_att[2][1];
00271 cur->abs_att(2,2) = abs_att[2][2];
00272
00273
00274 }
00275 calc_abs_positions(cur->brother, sg);
00276 calc_abs_positions(cur->child, sg);
00277 }
00278
00279 void Chain::calc_rel_positions(Joint* cur, SceneGraph* sg)
00280 {
00281 if(!cur) return;
00282 TransformNode* tnode = sg->findTransformNode(cur->name);
00283
00284 if(cur->parent && tnode)
00285 {
00286 static fVec3 pp, rel_pos;
00287 static fMat33 tr, rel_att;
00288 pp.sub(cur->abs_pos, cur->parent->abs_pos);
00289 tr.tran(cur->parent->abs_att);
00290 rel_pos.mul(tr, pp);
00291 rel_att.mul(tr, cur->abs_att);
00292
00293 cur->rel_pos.set(rel_pos);
00294 cur->rel_att.set(rel_att);
00295 cur->rel_ep.set(rel_att);
00296
00297 cur->init_pos.set(rel_pos);
00298 cur->init_att.set(rel_att);
00299
00300
00301
00302 }
00303 calc_rel_positions(cur->brother, sg);
00304 calc_rel_positions(cur->child, sg);
00305 }
00306 #endif