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
00017
00018
00019 #ifdef SEGA
00020 int Chain::init()
00021 #else
00022 int Chain::init(SceneGraph* sg)
00023 #endif
00024 {
00025
00026 if(all_value) delete[] all_value;
00027 if(all_value_dot) delete[] all_value_dot;
00028 if(all_vel) delete[] all_vel;
00029 if(all_vel_dot) delete[] all_vel_dot;
00030 if(j_acc_p[0]) delete[] j_acc_p[0];
00031 if(j_acc_p[1]) delete[] j_acc_p[1];
00032 if(j_acc_p[2]) delete[] j_acc_p[2];
00033 if(j_acc_p[3]) delete[] j_acc_p[3];
00034 if(j_value_dot[0]) delete[] j_value_dot[0];
00035 if(j_value_dot[1]) delete[] j_value_dot[1];
00036 if(j_value_dot[2]) delete[] j_value_dot[2];
00037 if(j_value_dot[3]) delete[] j_value_dot[3];
00038 if(init_value) delete[] init_value;
00039 if(init_vel) delete[] init_vel;
00040 n_value = 0;
00041 n_dof = 0;
00042 n_thrust = 0;
00043 n_joint = 0;
00044 all_value = 0;
00045 all_value_dot = 0;
00046 all_vel = 0;
00047 all_vel_dot = 0;
00048 j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
00049 j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
00050 init_value = 0;
00051 init_vel = 0;
00052 if(!root) return 0;
00053
00054 #ifndef SEGA
00055 if(sg) set_relative_positions(sg);
00056 #endif
00057 root->init();
00058 if(n_value > 0)
00059 {
00060 all_value = new double* [n_value];
00061 all_value_dot = new double* [n_value];
00062 j_value_dot[0] = new double [n_value];
00063 j_value_dot[1] = new double [n_value];
00064 j_value_dot[2] = new double [n_value];
00065 j_value_dot[3] = new double [n_value];
00066 init_value = new double [n_value];
00067 }
00068 if(n_dof > 0)
00069 {
00070 all_vel = new double* [n_dof];
00071 all_vel_dot = new double* [n_dof];
00072 j_acc_p[0] = new double [n_dof];
00073 j_acc_p[1] = new double [n_dof];
00074 j_acc_p[2] = new double [n_dof];
00075 j_acc_p[3] = new double [n_dof];
00076 init_vel = new double [n_dof];
00077 }
00078 root->init_arrays();
00079 CalcPosition();
00080 root->init_virtual();
00081 #ifndef SEGA
00082 if(sg)
00083 {
00084 init_scale(sg);
00085 }
00086 apply_scale();
00087 #endif
00088 return 0;
00089 }
00090
00091 #ifndef SEGA
00092 void Chain::init_scale(SceneGraph* sg)
00093 {
00094 init_scale_sub(sg->getNodes());
00095 }
00096
00097 void Chain::init_scale_sub(Node* node)
00098 {
00099 if(!node) return;
00100 char* name = 0;
00101 if(node->isTransformNode() && (name = node->getName()) && strstr(name, ScaleString))
00102 {
00103 char* jointname = name + strlen(ScaleString);
00104 float scale[3];
00105 ((TransformNode*)node)->getScale(scale);
00106 scale_object _s;
00107 _s.set_joint_name(jointname, 0);
00108 _s.scale = scale[0];
00109 add_scale_object(_s);
00110 }
00111 init_scale_sub(node->next());
00112 init_scale_sub(node->getChildNodes());
00113 }
00114
00115 void Chain::ApplyGeomScale(SceneGraph* sg)
00116 {
00117 apply_geom_scale(sg, root);
00118 }
00119
00120 void Chain::apply_geom_scale(SceneGraph* sg, Joint* cur)
00121 {
00122 if(!cur) return;
00123 TransformNode* mytrans = sg->findTransformNode(cur->name);
00124
00125 if(mytrans)
00126 {
00127 float new_scale = cur->cur_scale;
00128
00129 }
00130 apply_geom_scale(sg, cur->brother);
00131 apply_geom_scale(sg, cur->child);
00132 }
00133 #endif
00134
00135 void Joint::init()
00136 {
00137
00138 {
00139 if(parent) n_root_dof = parent->n_root_dof;
00140 if(t_given)
00141 {
00142 i_value = chain->n_value;
00143 i_dof = chain->n_dof;
00144 i_thrust = -1;
00145 n_root_dof += n_dof;
00146 }
00147 else
00148 {
00149 i_value = -1;
00150 i_dof = -1;
00151 i_thrust = chain->n_thrust;
00152 n_root_dof += n_thrust;
00153 }
00154 i_joint = chain->n_joint;
00155 chain->n_joint++;
00156 switch(j_type)
00157 {
00158 case JROTATE:
00159 case JSLIDE:
00160 if(t_given)
00161 {
00162 chain->n_value++;
00163 chain->n_dof++;
00164 }
00165 else
00166 {
00167 chain->n_thrust++;
00168 }
00169 break;
00170 case JSPHERE:
00171 if(t_given)
00172 {
00173 chain->n_value += 4;
00174 chain->n_dof += 3;
00175 }
00176 else
00177 {
00178 chain->n_thrust += 3;
00179 }
00180 break;
00181 case JFREE:
00182 if(t_given)
00183 {
00184 chain->n_value += 7;
00185 chain->n_dof += 6;
00186 }
00187 else
00188 {
00189 chain->n_thrust += 6;
00190 }
00191 break;
00192 case JFIXED:
00193 break;
00194 default:
00195 cerr << "warning: joint type not set for " << name << endl;
00196 break;
00197 }
00198
00199 }
00200 child->init();
00201 brother->init();
00202 }
00203
00204 void Joint::init_arrays()
00205 {
00206
00207 {
00208
00209 int i;
00210 switch(j_type)
00211 {
00212 case JROTATE:
00213 case JSLIDE:
00214 if(t_given)
00215 {
00216 chain->all_value[i_value] = &q;
00217 chain->all_value_dot[i_value] = &qd;
00218 chain->all_vel[i_dof] = &qd;
00219 chain->all_vel_dot[i_dof] = &qdd;
00220 }
00221 break;
00222 case JSPHERE:
00223 if(t_given)
00224 {
00225 for(i=0; i<4; i++)
00226 {
00227 chain->all_value[i_value+i] = &rel_ep(i);
00228 chain->all_value_dot[i_value+i] = &p_ep_dot(i);
00229 }
00230 for(i=0; i<3; i++)
00231 {
00232 chain->all_vel[i_dof+i] = &p_ang_vel(i);
00233 chain->all_vel_dot[i_dof+i] = &p_ang_acc(i);
00234 }
00235 }
00236 break;
00237 case JFREE:
00238 if(t_given)
00239 {
00240 for(i=0; i<3; i++)
00241 {
00242 chain->all_value[i_value+i] = &rel_pos(i);
00243 chain->all_value_dot[i_value+i] = &p_lin_vel(i);
00244 }
00245 for(i=0; i<4; i++)
00246 {
00247 chain->all_value[i_value+3+i] = &rel_ep(i);
00248 chain->all_value_dot[i_value+3+i] = &p_ep_dot(i);
00249 }
00250 for(i=0; i<3; i++)
00251 {
00252 chain->all_vel[i_dof+i] = &p_lin_vel(i);
00253 chain->all_vel_dot[i_dof+i] = &p_lin_acc(i);
00254 }
00255 for(i=0; i<3; i++)
00256 {
00257 chain->all_vel[i_dof+3+i] = &p_ang_vel(i);
00258 chain->all_vel_dot[i_dof+3+i] = &p_ang_acc(i);
00259 }
00260 }
00261 break;
00262 default:
00263 break;
00264 }
00265
00266 }
00267 child->init_arrays();
00268 brother->init_arrays();
00269 }
00270
00271 void Joint::init_virtual()
00272 {
00273 _init_virtual();
00274 brother->init_virtual();
00275 child->init_virtual();
00276 }
00277
00278 void Joint::_init_virtual()
00279 {
00280 if(!realname) return;
00281 real = chain->FindJoint(realname);
00282 if(!real)
00283 {
00284 cerr << "warning: could not find real joint " << realname << " of " << name << endl;
00285 return;
00286 }
00287 if(real->real)
00288 {
00289 cerr << "error: real joint " << realname << " of " << name << " is also virtual" << endl;
00290 real = 0;
00291 return;
00292 }
00293
00294 fVec3 pp;
00295 fMat33 Rt, IR;
00296 Rt.tran(real->abs_att);
00297 pp.sub(abs_pos, real->abs_pos);
00298 rpos_real.mul(Rt, pp);
00299 ratt_real.mul(Rt, abs_att);
00300
00301 mass = real->mass;
00302 pp.sub(real->loc_com, rpos_real);
00303 loc_com.mul(pp, ratt_real);
00304 IR.mul(real->inertia, ratt_real);
00305 inertia.mul(tran(ratt_real), IR);
00306 #if 0 // debug
00307 cerr << "-- " << name << endl;
00308 cerr << "rpos_real = " << rpos_real << endl;
00309 cerr << "ratt_real = " << ratt_real << endl;
00310 cerr << "mass = " << mass << endl;
00311 cerr << "loc_com = " << loc_com << endl;
00312 cerr << "inertia = " << inertia << endl;
00313 #endif
00314 }