00001 #include <sys/param.h>
00002 #include <iostream>
00003 #include <boost/algorithm/string.hpp>
00004
00005 #include <libxml/parser.h>
00006 #include <libxml/xmlreader.h>
00007 #include <libxml/xpath.h>
00008 #include <libxml/xpathInternals.h>
00009 #include <hrpModel/Config.h>
00010 #include "Project.h"
00011
00012 ThreeDView::ThreeDView() :
00013 showScale(true), showCoM(false), showCoMonFloor(false), showCollision(true){
00014 double r = 5.0, pan = M_PI/4, tilt = M_PI/16;
00015 double cp = cos(pan), sp = sin(pan);
00016 double ct = cos(tilt), st = sin(tilt);
00017
00018 hrp::Matrix33 Rp = hrp::rotFromRpy(0,pan,0);
00019 hrp::Matrix33 Rt = hrp::rotFromRpy(-tilt, 0, 0);
00020 hrp::Matrix33 R = hrp::rotFromRpy(0, M_PI/2,0)*hrp::rotFromRpy(0, 0, M_PI/2)*Rp*Rt;
00021 T[ 0] = R(0,0); T[ 1] = R(0,1); T[ 2] = R(0,2); T[ 3] = r*cp*ct;
00022 T[ 4] = R(1,0); T[ 5] = R(1,1); T[ 6] = R(1,2); T[ 7] = r*sp*ct;
00023 T[ 8] = R(2,0); T[ 9] = R(2,1); T[10] = R(2,2); T[11] = r*st + 0.8;
00024 T[12] = 0; T[13] = 0; T[14] = 0; T[15] = 1.0;
00025 }
00026
00027 Project::Project() :
00028 m_timeStep(0.001), m_logTimeStep(0.01), m_totalTime(1.0), m_gravity(9.8), m_isEuler(true), m_kinematicsOnly(false), m_realTime(false)
00029 {
00030 }
00031
00032 bool Project::parse(const std::string& filename)
00033 {
00034 xmlInitParser();
00035 xmlDocPtr doc = xmlParseFile(filename.c_str());
00036 if ( doc == NULL ) {
00037 std::cerr << "unable to parse file(" << filename << ")" << std::endl;
00038 return false;
00039 }
00040
00041
00042 xmlXPathContextPtr xpathCtx = xmlXPathNewContext(doc);
00043 if ( xpathCtx == NULL ) {
00044 std::cerr << "unable to create new XPath context" << std::endl;
00045 xmlFreeDoc(doc);
00046 return false;
00047 }
00048
00049
00050 xmlXPathObjectPtr xpathObj = xmlXPathEvalExpression(BAD_CAST "/grxui/mode/item", xpathCtx);
00051 if ( xmlXPathNodeSetIsEmpty(xpathObj->nodesetval) ) {
00052 std::cerr << "unable to find <mode>" << std::endl;
00053 }
00054
00055 int size;
00056 size = xpathObj->nodesetval->nodeNr;
00057
00058 for (int i = 0; i < size; i++ ) {
00059 xmlNodePtr node = xpathObj->nodesetval->nodeTab[i];
00060
00061 if ( xmlStrEqual( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.item.GrxSimulationItem") ) {
00062 xmlNodePtr cur_node = node->children;
00063 while ( cur_node ) {
00064 if ( cur_node->type == XML_ELEMENT_NODE ) {
00065 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"integrate") ) {
00066 m_kinematicsOnly = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == "false";
00067
00068 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"viewsimulate") ) {
00069 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"totalTime") ) {
00070 m_totalTime = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00071 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"timeStep") ) {
00072 m_timeStep = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00073 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"realTime") ) {
00074 m_realTime = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == "true";
00075 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"gravity") ) {
00076 m_gravity = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00077 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"method") ) {
00078 m_isEuler = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == std::string("EULER");
00079 } else {
00080 #if 0
00081 std::cerr << "Unknown tag : " << cur_node->name << " "
00082 << "name=" << xmlGetProp(cur_node, (xmlChar *)"name")
00083 << "value=" << xmlGetProp(cur_node, (xmlChar *)"value") << std::endl;
00084 #endif
00085 }
00086 }
00087 cur_node = cur_node->next;
00088 }
00089 } else if ( xmlStrEqual( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.item.GrxRTSItem") ) {
00090 xmlNodePtr cur_node = node->children;
00091 while ( cur_node ) {
00092 if ( cur_node->type == XML_ELEMENT_NODE ) {
00093 std::string name = (char *)xmlGetProp(cur_node, (xmlChar *)"name");
00094 std::string value = (char *)xmlGetProp(cur_node, (xmlChar *)"value");
00095
00096
00097 if (name == "connection"){
00098 int pos = value.find(':');
00099 if (pos < 0){
00100 std::cerr << "can't find a separator(:) in "
00101 << value << std::endl;
00102 }else{
00103 std::string p1 = value.substr(0,pos);
00104 std::string p2 = value.substr(pos+1);
00105 m_rts.connections.push_back(std::make_pair(p1, p2));
00106 }
00107 }else{
00108 int pos = name.find('.');
00109 if (pos < 0){
00110 std::cerr << "unknown property name:" << name
00111 << std::endl;
00112 }else{
00113 std::string comp = name.substr(0,pos);
00114 std::string cat = name.substr(pos+1);
00115 RTSItem::rtc &rtc = m_rts.components[comp];
00116 if (cat == "factory"){
00117 rtc.path = value;
00118 int pos = value.find_last_of("/");
00119 rtc.name = value.substr(pos+1);
00120 }else if (cat == "period") {
00121 rtc.period = atof(value.c_str());
00122 }else{
00123 rtc.configuration.push_back(std::make_pair(cat, value));
00124 }
00125 }
00126 }
00127 }
00128 cur_node = cur_node->next;
00129 }
00130 } else if ( xmlStrEqual( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.item.GrxModelItem") ) {
00131
00132 std::string path = (char *)xmlGetProp(node, (xmlChar *)"url");
00133 if ( path.find("$(CURRENT_DIR)") != std::string::npos ) {
00134 if (filename.find_last_of("/") != std::string::npos){
00135 path.replace(path.find("$(CURRENT_DIR)"),14,
00136 filename.substr(0, filename.find_last_of("/")));
00137 }else{
00138 path.replace(path.find("$(CURRENT_DIR)"),15, "");
00139 }
00140 if (path[0] != '/'){
00141 char buf[MAXPATHLEN];
00142 path = std::string(getcwd(buf, MAXPATHLEN))+"/"+path;
00143 }
00144 }
00145 if ( path.find("$(PROJECT_DIR)") != std::string::npos ) {
00146 std::string shdir = OPENHRP_SHARE_DIR;
00147 std::string pjdir = shdir + "/sample/project";
00148 path.replace(path.find("$(PROJECT_DIR)"),14, pjdir);
00149 }
00150 ModelItem m;
00151 m.url = std::string("file://")+path;
00152 xmlNodePtr cur_node = node->children;
00153 while ( cur_node ) {
00154 if ( cur_node->type == XML_ELEMENT_NODE ) {
00155 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"isRobot") ) {
00156 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"value"),(xmlChar *)"true")) {
00157
00158 }
00159 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"controlTime") ) {
00160
00161 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"rtcName") ) {
00162 m.rtcName = (char *)xmlGetProp(cur_node, (xmlChar *)"value");
00163 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"inport") ) {
00164 m.inports.push_back((char *)xmlGetProp(cur_node, (xmlChar *)"value"));
00165 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"outport") ) {
00166 m.outports.push_back((char *)xmlGetProp(cur_node, (xmlChar *)"value"));
00167 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".angle") != std::string::npos ) {
00168 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00169 name.erase(name.rfind(".angle"));
00170 m.joint[name].angle = atof((char *)xmlGetProp(cur_node, (xmlChar *)"value"));
00171 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".mode") != std::string::npos ) {
00172 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00173 name.erase(name.rfind(".mode"));
00174 m.joint[name].isHighGain = xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"value"), (xmlChar *)"HighGain");
00175 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".NumOfAABB") != std::string::npos ) {
00176 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00177 name.erase(name.rfind(".NumOfAABB"));
00178 m.joint[name].NumOfAABB = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00179 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".collisionShape") != std::string::npos ) {
00180 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00181 name.erase(name.rfind(".collisionShape"));
00182 m.joint[name].collisionShape = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00183 boost::trim(m.joint[name].collisionShape);
00184 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".translation") != std::string::npos ) {
00185 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00186 name.erase(name.rfind(".translation"));
00187 float x, y, z;
00188 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%f %f %f", &x, &y, &z);
00189 m.joint[name].translation[0] = x; m.joint[name].translation[1] = y; m.joint[name].translation[2] = z;
00190 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".rotation") != std::string::npos ) {
00191 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00192 name.erase(name.rfind(".rotation"));
00193 float x, y, z, w;
00194 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%f %f %f %f", &x, &y, &z, &w);
00195 hrp::calcRodrigues(m.joint[name].rotation, hrp::Vector3(x, y, z), w);
00196 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".velocity") != std::string::npos ) {
00197 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00198 name.erase(name.rfind(".velocity"));
00199 float x, y, z;
00200 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%f %f %f", &x, &y, &z);
00201 m.joint[name].linearVelocity << x, y, z;
00202 } else if ( std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name")).rfind(".angularVelocity") != std::string::npos ) {
00203 std::string name = std::string((char *)xmlGetProp(cur_node, (xmlChar *)"name"));
00204 name.erase(name.rfind(".angularVelocity"));
00205 float x, y, z;
00206 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%f %f %f", &x, &y, &z);
00207 m.joint[name].angularVelocity << x, y, z;
00208 } else {
00209 #if 0
00210 std::cerr << "Unknown tag : " << cur_node->name << " "
00211 << "name=" << xmlGetProp(cur_node, (xmlChar *)"name") << " "
00212 << "value=" << xmlGetProp(cur_node, (xmlChar *)"value") << std::endl;
00213 #endif
00214 }
00215 }
00216 cur_node = cur_node->next;
00217 }
00218 std::string n = std::string((char *)xmlGetProp(node, (xmlChar *)"name"));
00219 m_models[n] = m;
00220 } else if ( xmlStrEqual( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.item.GrxWorldStateItem") ) {
00221 xmlNodePtr cur_node = node->children;
00222 while ( cur_node ) {
00223 if ( cur_node->type == XML_ELEMENT_NODE ) {
00224 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"logTimeStep") ) {
00225 m_logTimeStep = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00226 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"timeStep") ) {
00227 m_timeStep = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00228 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"totalTime") ) {
00229 m_totalTime = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00230 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"method") ) {
00231 m_isEuler = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == std::string("EULER");
00232 }
00233 }
00234 cur_node = cur_node->next;
00235 }
00236 } else if ( xmlStrEqual ( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.item.GrxCollisionPairItem") ) {
00237 CollisionPairItem c;
00238 xmlNodePtr cur_node = node->children;
00239 while ( cur_node ) {
00240 if ( cur_node->type == XML_ELEMENT_NODE ) {
00241 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"objectName1") ) {
00242 c.objectName1 = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00243 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"objectName2") ) {
00244 c.objectName2 = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00245 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"jointName1") ) {
00246 c.jointName1 = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00247 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"jointName2") ) {
00248 c.jointName2 = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00249 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"slidingFriction") ) {
00250 c.slidingFriction = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00251 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"staticFriction") ) {
00252 c.staticFriction = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00253 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"cullingThresh") ) {
00254 c.cullingThresh = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00255 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"Restitution") ) {
00256 c.restitution = atof((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00257 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"sprintDamperModel") ) {
00258 c.sprintDamperModel = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00259 } else {
00260 #if 0
00261 std::cerr << "Unknown tag : " << cur_node->name << " "
00262 << ", name=" << xmlGetProp(cur_node, (xmlChar *)"name")
00263 << ", value=" << xmlGetProp(cur_node, (xmlChar *)"value") << std::endl;
00264 #endif
00265 }
00266 }
00267 cur_node = cur_node->next;
00268 }
00269 m_collisionPairs.push_back(c);
00270 } else if ( xmlStrEqual ( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.item.GrxExtraJointItem") ) {
00271 ExtraJointItem c;
00272 xmlNodePtr cur_node = node->children;
00273 while ( cur_node ) {
00274 if ( cur_node->type == XML_ELEMENT_NODE ) {
00275 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"object1Name") ) {
00276 c.object1Name = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00277 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"object2Name") ) {
00278 c.object2Name = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00279 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"link1Name") ) {
00280 c.link1Name = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00281 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"link2Name") ) {
00282 c.link2Name = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00283 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"jointType") ) {
00284 c.jointType = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00285 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"jointAxis") ) {
00286 float x, y, z;
00287 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%f %f %f", &x, &y, &z);
00288 c.jointAxis[0] = x; c.jointAxis[1] = y; c.jointAxis[2] = z;
00289 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"link1LocalPos") ) {
00290 float x, y, z;
00291 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%f %f %f", &x, &y, &z);
00292 c.link1LocalPos[0] = x; c.link1LocalPos[1] = y; c.link1LocalPos[2] = z;
00293 } else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"link2LocalPos") ) {
00294 float x, y, z;
00295 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%f %f %f", &x, &y, &z);
00296 c.link2LocalPos[0] = x; c.link2LocalPos[1] = y; c.link2LocalPos[2] = z;
00297 } else {
00298 #if 0
00299 std::cerr << "Unknown tag : " << cur_node->name << " "
00300 << ", name=" << xmlGetProp(cur_node, (xmlChar *)"name")
00301 << ", value=" << xmlGetProp(cur_node, (xmlChar *)"value") << std::endl;
00302 #endif
00303 }
00304 }
00305 cur_node = cur_node->next;
00306 }
00307 m_extraJoints.push_back(c);
00308 }
00309 }
00310
00311
00312 xmlXPathFreeObject(xpathObj);
00313 xmlXPathFreeContext(xpathCtx);
00314
00315 {
00316
00317 xmlXPathContextPtr xpathCtx = xmlXPathNewContext(doc);
00318 if ( xpathCtx == NULL ) {
00319 std::cerr << "unable to create new XPath context" << std::endl;
00320 xmlFreeDoc(doc);
00321 return false;
00322 }
00323
00324 xmlXPathObjectPtr xpathObj = xmlXPathEvalExpression(BAD_CAST "/grxui/mode/view", xpathCtx);
00325 if ( xmlXPathNodeSetIsEmpty(xpathObj->nodesetval) ) {
00326 std::cerr << "unable to find <mode>" << std::endl;
00327 }
00328
00329 int size;
00330 size = xpathObj->nodesetval->nodeNr;
00331
00332 for (int i = 0; i < size; i++ ) {
00333 xmlNodePtr node = xpathObj->nodesetval->nodeTab[i];
00334
00335 if ( xmlStrEqual( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.view.GrxRobotHardwareClientView") ) {
00336 xmlNodePtr cur_node = node->children;
00337 while ( cur_node ) {
00338 if ( cur_node->type == XML_ELEMENT_NODE ) {
00339 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"robotHost") ) {
00340 m_rhview.hostname = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00341 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"robotPort") ) {
00342 m_rhview.port = atoi((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00343 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"interval") ) {
00344 m_rhview.interval = atoi((char *)(xmlGetProp(cur_node, (xmlChar *)"value")));
00345 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"stateProvider") ) {
00346 m_rhview.RobotHardwareName = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00347 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"StateHolderRTC") ) {
00348 m_rhview.StateHolderName = (char *)(xmlGetProp(cur_node, (xmlChar *)"value"));
00349 }
00350 }
00351 cur_node = cur_node->next;
00352 }
00353 } else if ( xmlStrEqual( xmlGetProp(node, (xmlChar *)"class"), (xmlChar *)"com.generalrobotix.ui.view.Grx3DView") ) {
00354 xmlNodePtr cur_node = node->children;
00355 while ( cur_node ) {
00356 if ( cur_node->type == XML_ELEMENT_NODE ) {
00357 if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"showScale") ) {
00358 m_3dview.showScale = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == "true";
00359 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"showCoM") ) {
00360 m_3dview.showCoM = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == "true";
00361 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"showCoMonFloor") ) {
00362 m_3dview.showCoMonFloor = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == "true";
00363 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"showCollision") ) {
00364 m_3dview.showCollision = std::string((char *)(xmlGetProp(cur_node, (xmlChar *)"value"))) == "true";
00365 }else if ( xmlStrEqual(xmlGetProp(cur_node, (xmlChar *)"name"),(xmlChar *)"eyeHomePosition") ) {
00366 sscanf(((char *)xmlGetProp(cur_node, (xmlChar *)"value")),"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf ",
00367 &m_3dview.T[0],&m_3dview.T[1],&m_3dview.T[2],&m_3dview.T[3],
00368 &m_3dview.T[4],&m_3dview.T[5],&m_3dview.T[6],&m_3dview.T[7],
00369 &m_3dview.T[8],&m_3dview.T[9],&m_3dview.T[10],&m_3dview.T[11]);
00370 }
00371 }
00372 cur_node = cur_node->next;
00373 }
00374 }
00375 }
00376 xmlXPathFreeObject(xpathObj);
00377 xmlXPathFreeContext(xpathCtx);
00378 }
00379
00380
00381 xmlFreeDoc(doc);
00382 xmlCleanupParser();
00383
00384 return true;
00385 }
00386