openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
libopenrave.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 
19 #include <boost/scoped_ptr.hpp>
20 #include <boost/utility.hpp>
21 #include <boost/thread/once.hpp>
22 
23 #include <streambuf>
24 
25 #ifndef _WIN32
26 #include <sys/stat.h>
27 #include <sys/types.h>
28 #endif
29 
30 #include <locale>
31 
32 #include "plugindatabase.h"
33 
34 #include <boost/lexical_cast.hpp>
35 
36 #define LIBXML_SAX1_ENABLED
37 #include <libxml/globals.h>
38 #include <libxml/xmlerror.h>
39 #include <libxml/parser.h>
40 #include <libxml/parserInternals.h> // only for xmlNewInputFromFile()
41 #include <libxml/tree.h>
42 
43 #include <libxml/debugXML.h>
44 #include <libxml/xmlmemory.h>
45 
46 namespace OpenRAVE {
47 
48 #ifdef _WIN32
49 const char s_filesep = '\\';
50 #else
51 const char s_filesep = '/';
52 #endif
53 
54 #if defined(USE_CRLIBM)
55 
56 #ifdef HAS_FENV_H
57 #include <fenv.h>
58 #endif
59 
60 #ifdef LIBM_ACCURACY_RESULTS_H
61 #include LIBM_ACCURACY_RESULTS_H
62 #endif
63 
64 // use round-nearest versions since it is the default
65 #ifdef LIBM_EXP_ACCURATE
66 dReal RaveExp(dReal f) {
67  return exp(f);
68 }
69 #else
70 dReal RaveExp(dReal f) {
71  return exp_rn(f);
72 }
73 #endif
74 #ifdef LIBM_LOG_ACCURATE
75 dReal RaveLog(dReal f) {
76  return log(f);
77 }
78 #else
79 dReal RaveLog(dReal f) {
80  return log_rn(f);
81 }
82 #endif
83 #ifdef LIBM_COS_ACCURATE
84 dReal RaveCos(dReal f) {
85  return cos(f);
86 }
87 #else
88 dReal RaveCos(dReal f) {
89  return cos_rn(f);
90 }
91 #endif
92 #ifdef LIBM_SIN_ACCURATE
93 dReal RaveSin(dReal f) {
94  return sin(f);
95 }
96 #else
97 dReal RaveSin(dReal f) {
98  return sin_rn(f);
99 }
100 #endif
101 #ifdef LIBM_TAN_ACCURATE
102 dReal RaveTan(dReal f) {
103  return tan(f);
104 }
105 #else
106 dReal RaveTan(dReal f) {
107  return tan_rn(f);
108 }
109 #endif
110 #ifdef LIBM_LOG2_ACCURATE
111 dReal RaveLog2(dReal f) {
112  return log2(f);
113 }
114 #else
115 dReal RaveLog2(dReal f) {
116  return log2_rn(f);
117 }
118 #endif
119 #ifdef LIBM_LOG10_ACCURATE
120 dReal RaveLog10(dReal f) {
121  return log10(f);
122 }
123 #else
124 dReal RaveLog10(dReal f) {
125  return log10_rn(f);
126 }
127 #endif
128 #ifdef LIBM_ACOS_ACCURATE
129 dReal RaveAcos(dReal f) {
130  return acos(f);
131 }
132 #else
133 dReal RaveAcos(dReal f) {
134  return acos_rn(f);
135 }
136 #endif
137 #ifdef LIBM_ASIN_ACCURATE
138 dReal RaveAsin(dReal f) {
139  return asin(f);
140 }
141 #else
142 dReal RaveAsin(dReal f) {
143  return asin_rn(f);
144 }
145 #endif
146 #ifdef LIBM_ATAN2_ACCURATE
147 dReal RaveAtan2(dReal y, dReal x) {
148  return atan2(y,x);
149 }
150 #else
151 dReal RaveAtan2(dReal y, dReal x) // unfortunately no atan2 in crlibm...
152 {
153  dReal absx, absy, val;
154  if ((x == 0)&&(y == 0)) {
155  return 0;
156  }
157  absy = y < 0 ? -y : y;
158  absx = x < 0 ? -x : x;
159  if (absy - absx == absy) {
160  // x negligible compared to y
161  return y < 0 ? -M_PI_2 : M_PI_2;
162  }
163  if (absx - absy == absx) {
164  // y negligible compared to x
165  val = 0.0;
166  }
167  else val = atan_rn(y/x);
168  if (x > 0) {
169  // first or fourth quadrant; already correct
170  return val;
171  }
172  if (y < 0) {
173  // third quadrant
174  return val - M_PI;
175  }
176  return val + M_PI;
177 }
178 #endif
179 #ifdef LIBM_POW_ACCURATE
180 dReal RavePow(dReal x, dReal y) {
181  return pow(x,y);
182 }
183 #else
184 dReal RavePow(dReal x, dReal y) {
185  return pow_rn(x,y);
186 }
187 #endif
188 #ifdef LIBM_SQRT_ACCURATE
189 dReal RaveSqrt(dReal f) {
190  return sqrt(f);
191 }
192 #else
193 dReal RaveSqrt(dReal f) {
194  return sqrt(f);
195 }
196 //dReal RaveSqrt(dReal f) { return pow_rn(f,0.5); } // NOTE: this is really slow, is it really worth the precision?
197 #endif
198 dReal RaveFabs(dReal f) {
199  return fabs(f);
200 }
201 
202 #else // use all standard libm
203 
204 #if OPENRAVE_PRECISION == 0 // floating-point
206  return expf(f);
207 }
209  return logf(f);
210 }
212  return cosf(f);
213 }
215  return sinf(f);
216 }
218  return tanf(f);
219 }
220 #ifdef HAS_LOG2
221 dReal RaveLog2(dReal f) {
222  return log2f(f);
223 }
224 #else
226  return logf(f)/logf(2.0f);
227 }
228 #endif
230  return log10f(f);
231 }
233  return acosf(f);
234 }
236  return asinf(f);
237 }
239  return atan2f(fy,fx);
240 }
242  return powf(fx,fy);
243 }
245  return sqrtf(f);
246 }
248  return fabsf(f);
249 }
250 #else
251 dReal RaveExp(dReal f) {
252  return exp(f);
253 }
254 dReal RaveLog(dReal f) {
255  return log(f);
256 }
257 dReal RaveCos(dReal f) {
258  return cos(f);
259 }
260 dReal RaveSin(dReal f) {
261  return sin(f);
262 }
263 dReal RaveTan(dReal f) {
264  return tan(f);
265 }
266 #ifdef HAS_LOG2
267 dReal RaveLog2(dReal f) {
268  return log2(f);
269 }
270 #else
271 dReal RaveLog2(dReal f) {
272  return log(f)/log(2.0f);
273 }
274 #endif
275 dReal RaveLog10(dReal f) {
276  return log10(f);
277 }
278 dReal RaveAcos(dReal f) {
279  return acos(f);
280 }
281 dReal RaveAsin(dReal f) {
282  return asin(f);
283 }
284 dReal RaveAtan2(dReal fy, dReal fx) {
285  return atan2(fy,fx);
286 }
287 dReal RavePow(dReal fx, dReal fy) {
288  return pow(fx,fy);
289 }
290 dReal RaveSqrt(dReal f) {
291  return sqrt(f);
292 }
293 dReal RaveFabs(dReal f) {
294  return fabs(f);
295 }
296 #endif
297 
298 #endif
299 
300 static boost::once_flag _onceRaveInitialize = BOOST_ONCE_INIT;
301 
304 class RaveGlobal : private boost::noncopyable, public boost::enable_shared_from_this<RaveGlobal>, public UserData
305 {
306  typedef std::map<std::string, CreateXMLReaderFn, CaseInsensitiveCompare> READERSMAP;
307 
308  RaveGlobal()
309  {
310  // is this really necessary? just makes bugs hard to reproduce...
311  //srand(GetMilliTime());
312  //RaveInitRandomGeneration(GetMilliTime());
313  _nDebugLevel = Level_Info;
314  _nGlobalEnvironmentId = 0;
315  _nDataAccessOptions = 0;
316 
317  _mapinterfacenames[PT_Planner] = "planner";
318  _mapinterfacenames[PT_Robot] = "robot";
319  _mapinterfacenames[PT_SensorSystem] = "sensorsystem";
320  _mapinterfacenames[PT_Controller] = "controller";
321  _mapinterfacenames[PT_Module] = "module";
322  _mapinterfacenames[PT_IkSolver] = "iksolver";
323  _mapinterfacenames[PT_KinBody] = "kinbody";
324  _mapinterfacenames[PT_PhysicsEngine] = "physicsengine";
325  _mapinterfacenames[PT_Sensor] = "sensor";
326  _mapinterfacenames[PT_CollisionChecker] = "collisionchecker";
327  _mapinterfacenames[PT_Trajectory] = "trajectory";
328  _mapinterfacenames[PT_Viewer] = "viewer";
329  _mapinterfacenames[PT_SpaceSampler] = "spacesampler";
330  BOOST_ASSERT(_mapinterfacenames.size()==PT_NumberOfInterfaces);
331 
332  _mapikparameterization[IKP_Transform6D] = "Transform6D";
333  _mapikparameterization[IKP_Rotation3D] = "Rotation3D";
334  _mapikparameterization[IKP_Translation3D] = "Translation3D";
335  _mapikparameterization[IKP_Direction3D] = "Direction3D";
336  _mapikparameterization[IKP_Ray4D] = "Ray4D";
337  _mapikparameterization[IKP_Lookat3D] = "Lookat3D";
338  _mapikparameterization[IKP_TranslationDirection5D] = "TranslationDirection5D";
339  _mapikparameterization[IKP_TranslationXY2D] = "TranslationXY2D";
340  _mapikparameterization[IKP_TranslationXYOrientation3D] = "TranslationXYOrientation3D";
341  _mapikparameterization[IKP_TranslationLocalGlobal6D] = "TranslationLocalGlobal6D";
342  _mapikparameterization[IKP_TranslationXAxisAngle4D] = "TranslationXAxisAngle4D";
343  _mapikparameterization[IKP_TranslationYAxisAngle4D] = "TranslationYAxisAngle4D";
344  _mapikparameterization[IKP_TranslationZAxisAngle4D] = "TranslationZAxisAngle4D";
345  _mapikparameterization[IKP_TranslationXAxisAngleZNorm4D] = "TranslationXAxisAngleZNorm4D";
346  _mapikparameterization[IKP_TranslationYAxisAngleXNorm4D] = "TranslationYAxisAngleXNorm4D";
347  _mapikparameterization[IKP_TranslationZAxisAngleYNorm4D] = "TranslationZAxisAngleYNorm4D";
348  BOOST_ASSERT(_mapikparameterization.size()==IKP_NumberOfParameterizations);
349  FOREACH(it,_mapikparameterization) {
350  std::string name = it->second;
351  std::transform(name.begin(), name.end(), name.begin(), ::tolower);
352  _mapikparameterizationlower[it->first] = name;
353  }
354  }
355 public:
356  virtual ~RaveGlobal() {
357  Destroy();
358  }
359 
360  static boost::shared_ptr<RaveGlobal>& instance()
361  {
362  boost::call_once(_create,_onceRaveInitialize);
363  return _state;
364  }
365 
366  int Initialize(bool bLoadAllPlugins, int level)
367  {
368  if( _IsInitialized() ) {
369  return 0; // already initialized
370  }
371 
372 #ifdef USE_CRLIBM
373  _crlibm_fpu_state = crlibm_init();
374 #endif
375  try {
376  // set to the classic locale so that number serialization/hashing works correctly
377  std::locale::global(std::locale::classic());
378  }
379  catch(const std::runtime_error& e) {
380  RAVELOG_WARN("failed to set to C locale: %s\n",e.what());
381  }
382 
383  _nDebugLevel = level;
384  _pdatabase.reset(new RaveDatabase());
385  if( !_pdatabase->Init(bLoadAllPlugins) ) {
386  RAVELOG_FATAL("failed to create the openrave plugin database\n");
387  }
388 
389  char* phomedir = getenv("OPENRAVE_HOME"); // getenv not thread-safe?
390  if( phomedir == NULL ) {
391 #ifndef _WIN32
392  _homedirectory = string(getenv("HOME"))+string("/.openrave"); // getenv not thread-safe?
393 #else
394  _homedirectory = string(getenv("HOMEDRIVE"))+string(getenv("HOMEPATH"))+string("\\.openrave"); // getenv not thread-safe?
395 #endif
396  }
397  else {
398  _homedirectory = phomedir;
399  }
400 #ifndef _WIN32
401  mkdir(_homedirectory.c_str(),S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH | S_IRWXU);
402 #else
403  CreateDirectory(_homedirectory.c_str(),NULL);
404 #endif
405 
406 #ifdef _WIN32
407  const char* delim = ";";
408 #else
409  const char* delim = ":";
410 #endif
411  _vdbdirectories.clear();
412  char* pOPENRAVE_PLUGINS = getenv("OPENRAVE_DATABASE"); // getenv not thread-safe?
413  if( pOPENRAVE_PLUGINS != NULL ) {
414  utils::TokenizeString(pOPENRAVE_PLUGINS, delim, _vdbdirectories);
415  }
416  _vdbdirectories.push_back(_homedirectory);
417 
418  _UpdateDataDirs();
419  return 0;
420  }
421 
422  void Destroy()
423  {
424  // don't use any log statements since global instance might be null
425  // environments have to be destroyed carefully since their destructors can be called, which will attempt to unregister the environment
426  std::map<int, EnvironmentBase*> mapenvironments;
427  {
428  boost::mutex::scoped_lock lock(_mutexinternal);
429  mapenvironments = _mapenvironments;
430  }
431  FOREACH(itenv,mapenvironments) {
432  // equire a shared pointer to prevent environment from getting deleted during Destroy loop
433  EnvironmentBasePtr penv = itenv->second->shared_from_this();
434  penv->Destroy();
435  }
436  mapenvironments.clear();
437  _mapenvironments.clear();
438  _pdefaultsampler.reset();
439  _mapreaders.clear();
440 
441  // process the callbacks
442  std::list<boost::function<void()> > listDestroyCallbacks;
443  {
444  boost::mutex::scoped_lock lock(_mutexinternal);
445  listDestroyCallbacks.swap(_listDestroyCallbacks);
446  }
447  FOREACH(itcallback, listDestroyCallbacks) {
448  (*itcallback)();
449  }
450  listDestroyCallbacks.clear();
451 
452  if( !!_pdatabase ) {
453  // force destroy in case some one is holding a pointer to it
454  _pdatabase->Destroy();
455  _pdatabase.reset();
456  }
457 #ifdef USE_CRLIBM
458 
459 #ifdef HAS_FENV_H
460  feclearexcept(-1); // clear any cached exceptions
461 #endif
462  crlibm_exit(_crlibm_fpu_state);
463 #endif
464  }
465 
466  void AddCallbackForDestroy(const boost::function<void()>& fn)
467  {
468  boost::mutex::scoped_lock lock(_mutexinternal);
469  _listDestroyCallbacks.push_back(fn);
470  }
471 
472  std::string GetHomeDirectory()
473  {
474  return _homedirectory;
475  }
476 
477  std::string FindDatabaseFile(const std::string& filename, bool bRead)
478  {
479  FOREACH(itdirectory,_vdbdirectories) {
480 #ifdef HAVE_BOOST_FILESYSTEM
481  std::string fullfilename = boost::filesystem::absolute(boost::filesystem::path(*itdirectory)/filename).string();
482 #else
483  std::string fullfilename = *itdirectory;
484  fullfilename += s_filesep;
485  fullfilename += filename;
486 #endif
487  if( bRead ) {
488  if( !!ifstream(fullfilename.c_str()) ) {
489  return fullfilename;
490  }
491  }
492  else {
493  return fullfilename;
494  }
495  }
496  return "";
497  }
498 
499  void SetDebugLevel(int level)
500  {
501  _nDebugLevel = level;
502  }
503 
505  {
506  return _nDebugLevel;
507  }
508 
510  {
511 public:
512  XMLReaderFunctionData(InterfaceType type, const std::string& xmltag, const CreateXMLReaderFn& fn, boost::shared_ptr<RaveGlobal> global) : _global(global), _type(type), _xmltag(xmltag)
513  {
514  boost::mutex::scoped_lock lock(global->_mutexinternal);
515  _oldfn = global->_mapreaders[_type][_xmltag];
516  global->_mapreaders[_type][_xmltag] = fn;
517  }
519  {
520  boost::shared_ptr<RaveGlobal> global = _global.lock();
521  if( !!global ) {
522  boost::mutex::scoped_lock lock(global->_mutexinternal);
523  global->_mapreaders[_type][_xmltag] = _oldfn;
524  }
525  }
526 protected:
528  boost::weak_ptr<RaveGlobal> _global;
529  InterfaceType _type;
530  std::string _xmltag;
531  };
532 
533  UserDataPtr RegisterXMLReader(InterfaceType type, const std::string& xmltag, const CreateXMLReaderFn& fn)
534  {
535  return UserDataPtr(new XMLReaderFunctionData(type,xmltag,fn,shared_from_this()));
536  }
537 
538  const BaseXMLReaderPtr CallXMLReader(InterfaceType type, const std::string& xmltag, InterfaceBasePtr pinterface, const AttributesList& atts)
539  {
540  READERSMAP::iterator it = _mapreaders[type].find(xmltag);
541  if( it == _mapreaders[type].end() ) {
542  //throw openrave_exception(str(boost::format("No function registered for interface %s xml tag %s")%GetInterfaceName(type)%xmltag),ORE_InvalidArguments);
543  return BaseXMLReaderPtr();
544  }
545  return it->second(pinterface,atts);
546  }
547 
548  boost::shared_ptr<RaveDatabase> GetDatabase() const {
549  return _pdatabase;
550  }
551  const std::map<InterfaceType,std::string>& GetInterfaceNamesMap() const {
552  return _mapinterfacenames;
553  }
554  const std::map<IkParameterizationType,std::string>& GetIkParameterizationMap(int alllowercase=0) {
555  if( alllowercase ) {
556  return _mapikparameterizationlower;
557  }
558  return _mapikparameterization;
559  }
560 
561  const std::string& GetInterfaceName(InterfaceType type)
562  {
563  std::map<InterfaceType,std::string>::const_iterator it = _mapinterfacenames.find(type);
564  if( it == _mapinterfacenames.end() ) {
565  throw OPENRAVE_EXCEPTION_FORMAT("Invalid type %d specified", type, ORE_Failed);
566  }
567  return it->second;
568  }
569 
570  // have to take in pointer instead of shared_ptr since method will be called in EnvironmentBase constructor
572  {
573  BOOST_ASSERT(!!_pdatabase);
574  boost::mutex::scoped_lock lock(_mutexinternal);
575  _mapenvironments[++_nGlobalEnvironmentId] = penv;
576  return _nGlobalEnvironmentId;
577  }
578 
580  {
581  boost::mutex::scoped_lock lock(_mutexinternal);
582  FOREACH(it, _mapenvironments) {
583  if( it->second == penv ) {
584  _mapenvironments.erase(it);
585  break;
586  }
587  }
588  }
589 
591  {
592  boost::mutex::scoped_lock lock(_mutexinternal);
593  FOREACH(it,_mapenvironments) {
594  if( it->second == penv.get() ) {
595  return it->first;
596  }
597  }
598  return 0;
599  }
600 
602  {
603  boost::mutex::scoped_lock lock(_mutexinternal);
604  std::map<int, EnvironmentBase*>::iterator it = _mapenvironments.find(id);
605  if( it == _mapenvironments.end() ) {
606  return EnvironmentBasePtr();
607  }
608  return it->second->shared_from_this();
609  }
610 
611  void GetEnvironments(std::list<EnvironmentBasePtr>& listenvironments)
612  {
613  listenvironments.clear();
614  boost::mutex::scoped_lock lock(_mutexinternal);
615  FOREACH(it,_mapenvironments) {
616  EnvironmentBasePtr penv = it->second->shared_from_this();
617  if( !!penv ) {
618  listenvironments.push_back(penv);
619  }
620  }
621  }
622 
624  {
625  if( !_pdefaultsampler ) {
626  boost::mutex::scoped_lock lock(_mutexinternal);
627  BOOST_ASSERT( _mapenvironments.size() > 0 );
628  _pdefaultsampler = GetDatabase()->CreateSpaceSampler(_mapenvironments.begin()->second->shared_from_this(),"MT19937");
629  }
630  return _pdefaultsampler;
631  }
632 
633  std::string FindLocalFile(const std::string& _filename, const std::string& curdir)
634  {
635 #ifndef HAVE_BOOST_FILESYSTEM
636  throw OPENRAVE_EXCEPTION_FORMAT0("need to compile with boost::filesystem",ORE_Assert);
637 #else
638  if( _filename.size() == 0 ) {
639  return std::string();
640  }
641 
642  boost::mutex::scoped_lock lock(_mutexinternal);
643  boost::filesystem::path fullfilename;
644  boost::filesystem::path filename(_filename);
645 
646  if( filename.is_complete() ) {
647  fullfilename = filename;
648  }
649  else if( curdir.size() > 0 ) {
650  fullfilename = boost::filesystem::absolute(boost::filesystem::path(curdir)) / filename;
651  }
652  else {
653  fullfilename = boost::filesystem::current_path() / filename;
654  }
655 
656  if( boost::filesystem::exists(fullfilename) ) {
657  if( !_ValidateFilename(fullfilename,boost::filesystem::path()) ) {
658  RAVELOG_WARN(str(boost::format("acess denied to file %s\n")%fullfilename));
659  return std::string();
660  }
661  return fullfilename.string();
662  }
663 
664  // try the openrave directories
665  FOREACHC(itdir, _vBoostDataDirs) {
666  fullfilename = *itdir / filename;
667  if( _ValidateFilename(fullfilename,boost::filesystem::path()) ) {
668  return fullfilename.string();
669  }
670  }
671 
672  RAVELOG_WARN(str(boost::format("could not find file %s\n")%filename));
673  return std::string();
674 #endif
675  }
676 
677  bool InvertFileLookup(std::string& newfilename, const std::string& filename)
678  {
679 #ifndef HAVE_BOOST_FILESYSTEM
680  RAVELOG_WARN("need to compile with boost::filesystem\n");
681 #else
682  // check if filename is within _vBoostDataDirs
683  boost::filesystem::path fullfilename = boost::filesystem::absolute(filename);
684  _CustomNormalizePath(fullfilename);
685  FOREACHC(itpath,_vBoostDataDirs) {
686  std::list<boost::filesystem::path> listfilenameparts;
687  boost::filesystem::path testfilename = fullfilename.parent_path();
688  while( testfilename >= *itpath ) {
689  if( testfilename == *itpath ) {
690  boost::filesystem::path relpath;
691  FOREACH(itpart,listfilenameparts) {
692  relpath /= *itpart;
693  }
694  relpath /= fullfilename.filename();
695  newfilename=relpath.string();
696  return true;
697  }
698  listfilenameparts.push_front(testfilename.filename());
699  testfilename = testfilename.parent_path();
700  }
701  }
702 #endif
703  return false;
704  }
705 
706  void SetDataAccess(int options) {
707  boost::mutex::scoped_lock lock(_mutexinternal);
708  _nDataAccessOptions = options;
709  }
711  boost::mutex::scoped_lock lock(_mutexinternal);
712  return _nDataAccessOptions;
713  }
714 
715 protected:
716  static void _create()
717  {
718  if( !_state ) {
719  _state.reset(new RaveGlobal());
720  }
721  }
722 
723  bool _IsInitialized() const {
724  return !!_pdatabase;
725  }
726 
728  {
729  _vdatadirs.resize(0);
730 
731  bool bExists=false;
732 #ifdef _WIN32
733  const char* delim = ";";
734 #else
735  const char* delim = ":";
736 #endif
737  char* pOPENRAVE_DATA = getenv("OPENRAVE_DATA"); // getenv not thread-safe?
738  if( pOPENRAVE_DATA != NULL ) {
739  utils::TokenizeString(pOPENRAVE_DATA, delim, _vdatadirs);
740  }
741  string installdir = OPENRAVE_DATA_INSTALL_DIR;
742 #ifdef HAVE_BOOST_FILESYSTEM
743  if( !boost::filesystem::is_directory(boost::filesystem::path(installdir)) ) {
744 #ifdef _WIN32
745  HKEY hkey;
746  if(RegOpenKeyEx(HKEY_LOCAL_MACHINE, TEXT("Software\\OpenRAVE\\" OPENRAVE_VERSION_STRING), 0, KEY_QUERY_VALUE, &hkey) == ERROR_SUCCESS) {
747  DWORD dwType = REG_SZ;
748  CHAR szInstallRoot[4096]; // dont' take chances, it is windows
749  DWORD dwSize = sizeof(szInstallRoot);
750  RegQueryValueEx(hkey, TEXT("InstallRoot"), NULL, &dwType, (PBYTE)szInstallRoot, &dwSize);
751  RegCloseKey(hkey);
752  installdir.assign(szInstallRoot);
753  installdir += str(boost::format("%cshare%copenrave-%d.%d")%s_filesep%s_filesep%OPENRAVE_VERSION_MAJOR%OPENRAVE_VERSION_MINOR);
754  RAVELOG_VERBOSE(str(boost::format("window registry data dir '%s'")%installdir));
755  }
756  else
757 #endif
758  {
759  RAVELOG_WARN(str(boost::format("%s doesn't exist")%installdir));
760  }
761  }
762 
763  boost::filesystem::path datafilename = boost::filesystem::absolute(boost::filesystem::path(installdir));
764  FOREACH(itname, _vdatadirs) {
765  if( datafilename == boost::filesystem::absolute(boost::filesystem::path(*itname)) ) {
766  bExists = true;
767  break;
768  }
769  }
770 #else
771  string datafilename=installdir;
772  FOREACH(itname, _vdatadirs) {
773  if( datafilename == installdir ) {
774  bExists = true;
775  break;
776  }
777  }
778 #endif
779  if( !bExists ) {
780  _vdatadirs.push_back(installdir);
781  }
782  FOREACHC(itdir,_vdatadirs) {
783  RAVELOG_VERBOSE(str(boost::format("data dir: %s")%*itdir));
784  }
785 
786 #ifdef HAVE_BOOST_FILESYSTEM
787  _vBoostDataDirs.resize(0);
788  FOREACHC(itfilename,_vdatadirs) {
789  boost::filesystem::path fullfilename = boost::filesystem::absolute(boost::filesystem::path(*itfilename));
790  _CustomNormalizePath(fullfilename);
791  if( fullfilename.filename() == "." ) {
792  // fullfilename ends in '/', so remove it
793  fullfilename = fullfilename.parent_path();
794  }
795  _vBoostDataDirs.push_back(fullfilename);
796  }
797 #endif
798  }
799 
800 #ifdef HAVE_BOOST_FILESYSTEM
801 
802  void _CustomNormalizePath(boost::filesystem::path& p)
803  {
804 #ifndef BOOST_FILESYSTEM_NO_DEPRECATED
805  p.normalize();
806 #else
807  boost::filesystem::path result;
808  for(boost::filesystem::path::iterator it=p.begin(); it!=p.end(); ++it)
809  {
810  if(*it == "..") {
811  // /a/b/.. is not necessarily /a if b is a symbolic link
812  if(boost::filesystem::is_symlink(result) ) {
813  result /= *it;
814  }
815  // /a/b/../.. is not /a/b/.. under most circumstances
816  // We can end up with ..s in our result because of symbolic links
817  else if(result.filename() == "..") {
818  result /= *it;
819  }
820  // Otherwise it should be safe to resolve the parent
821  else {
822  result = result.parent_path();
823  }
824  }
825  else if(*it == ".") {
826  // Ignore
827  }
828  else {
829  // Just cat other path entries
830  result /= *it;
831  }
832  }
833  p = result;
834 #endif
835  }
836 
837  bool _ValidateFilename(const boost::filesystem::path& filename, const boost::filesystem::path& curdir)
838  {
839  if( !boost::filesystem::exists(filename) ) {
840  return false;
841  }
842 
843  if( _nDataAccessOptions & 1 ) {
844  // check if filename is within _vBoostDataDirs
845  boost::filesystem::path fullfilename = boost::filesystem::absolute(filename,curdir.empty() ? boost::filesystem::current_path() : curdir);
846  _CustomNormalizePath(fullfilename);
847  bool bfound = false;
848  FOREACHC(itpath,_vBoostDataDirs) {
849  boost::filesystem::path testfilename = fullfilename.parent_path();
850  while( testfilename >= *itpath ) {
851  if( testfilename == *itpath ) {
852  bfound = true;
853  break;
854  }
855  testfilename = testfilename.parent_path();
856  }
857  if( bfound ) {
858  break;
859  }
860  }
861  if( !bfound ) {
862  return false;
863  }
864  }
865  return true;
866  }
867 
868 #endif
869 
870 private:
871  static boost::shared_ptr<RaveGlobal> _state;
872  // state that is always present
873 
874  // state that is initialized/destroyed
875  boost::shared_ptr<RaveDatabase> _pdatabase;
876  int _nDebugLevel;
877  boost::mutex _mutexinternal;
878  std::map<InterfaceType, READERSMAP > _mapreaders;
879  std::map<InterfaceType,string> _mapinterfacenames;
880  std::map<IkParameterizationType,string> _mapikparameterization, _mapikparameterizationlower;
881  std::map<int, EnvironmentBase*> _mapenvironments;
882  std::list<boost::function<void()> > _listDestroyCallbacks;
883  std::string _homedirectory;
884  std::vector<std::string> _vdbdirectories;
885  int _nGlobalEnvironmentId;
886  SpaceSamplerBasePtr _pdefaultsampler;
887 #ifdef USE_CRLIBM
888  long long _crlibm_fpu_state;
889 #endif
890  int _nDataAccessOptions;
891 
892  std::vector<string> _vdatadirs;
893 #ifdef HAVE_BOOST_FILESYSTEM
894  std::vector<boost::filesystem::path> _vBoostDataDirs;
895 #endif
896 
898  friend UserDataPtr RaveGlobalState();
899 };
900 
901 boost::shared_ptr<RaveGlobal> RaveGlobal::_state;
902 
903 void RaveSetDebugLevel(int level)
904 {
905  RaveGlobal::instance()->SetDebugLevel(level);
906 }
907 
909 {
910  return RaveGlobal::instance()->GetDebugLevel();
911 }
912 
913 const std::map<InterfaceType,std::string>& RaveGetInterfaceNamesMap()
914 {
915  return RaveGlobal::instance()->GetInterfaceNamesMap();
916 }
917 
918 const std::map<IkParameterizationType,std::string>& RaveGetIkParameterizationMap(int alllowercase)
919 {
920  return RaveGlobal::instance()->GetIkParameterizationMap(alllowercase);
921 }
922 
924 {
925  uniqueid &= IKP_UniqueIdMask;
926  FOREACHC(it, RaveGlobal::instance()->GetIkParameterizationMap()) {
927  if( (it->first & (IKP_UniqueIdMask&~IKP_VelocityDataBit)) == (uniqueid&(IKP_UniqueIdMask&~IKP_VelocityDataBit)) ) {
928  return static_cast<IkParameterizationType>(it->first|(uniqueid&IKP_VelocityDataBit));
929  }
930  }
931  throw OPENRAVE_EXCEPTION_FORMAT("no ik exists of unique id 0x%x",uniqueid,ORE_InvalidArguments);
932 }
933 
934 const std::string& RaveGetInterfaceName(InterfaceType type)
935 {
936  return RaveGlobal::instance()->GetInterfaceName(type);
937 }
938 
939 std::string RaveGetHomeDirectory()
940 {
941  return RaveGlobal::instance()->GetHomeDirectory();
942 }
943 
944 std::string RaveFindDatabaseFile(const std::string& filename, bool bRead)
945 {
946  return RaveGlobal::instance()->FindDatabaseFile(filename,bRead);
947 }
948 
949 int RaveInitialize(bool bLoadAllPlugins, int level)
950 {
951  return RaveGlobal::instance()->Initialize(bLoadAllPlugins,level);
952 }
953 
955 {
956  RaveGlobal::_state = boost::dynamic_pointer_cast<RaveGlobal>(globalstate);
957 }
958 
960 {
961  // only return valid pointer if initialized!
962  boost::shared_ptr<RaveGlobal> state = RaveGlobal::_state;
963  if( !!state && state->_IsInitialized() ) {
964  return state;
965  }
966  return UserDataPtr();
967 }
968 
970 {
971  RaveGlobal::instance()->Destroy();
972 }
973 
974 void RaveAddCallbackForDestroy(const boost::function<void()>& fn)
975 {
976  RaveGlobal::instance()->AddCallbackForDestroy(fn);
977 }
978 
980 {
981  return RaveGlobal::instance()->GetEnvironmentId(penv);
982 }
983 
985 {
986  return RaveGlobal::instance()->GetEnvironment(id);
987 }
988 
989 void RaveGetEnvironments(std::list<EnvironmentBasePtr>& listenvironments)
990 {
991  RaveGlobal::instance()->GetEnvironments(listenvironments);
992 }
993 
994 void RaveGetPluginInfo(std::list< std::pair<std::string, PLUGININFO> >& plugins)
995 {
996  RaveGlobal::instance()->GetDatabase()->GetPluginInfo(plugins);
997 }
998 
999 void RaveGetLoadedInterfaces(std::map<InterfaceType, std::vector<std::string> >& interfacenames)
1000 {
1001  RaveGlobal::instance()->GetDatabase()->GetLoadedInterfaces(interfacenames);
1002 }
1003 
1005 {
1006  RaveGlobal::instance()->GetDatabase()->ReloadPlugins();
1007 }
1008 
1009 bool RaveLoadPlugin(const std::string& libraryname)
1010 {
1011  return RaveGlobal::instance()->GetDatabase()->LoadPlugin(libraryname);
1012 }
1013 
1014 bool RaveHasInterface(InterfaceType type, const std::string& interfacename)
1015 {
1016  return RaveGlobal::instance()->GetDatabase()->HasInterface(type,interfacename);
1017 }
1018 
1019 InterfaceBasePtr RaveCreateInterface(EnvironmentBasePtr penv, InterfaceType type,const std::string& interfacename)
1020 {
1021  return RaveGlobal::instance()->GetDatabase()->Create(penv, type,interfacename);
1022 }
1023 
1024 RobotBasePtr RaveCreateRobot(EnvironmentBasePtr penv, const std::string& name)
1025 {
1026  return RaveGlobal::instance()->GetDatabase()->CreateRobot(penv,name);
1027 }
1028 
1030 {
1031  return RaveGlobal::instance()->GetDatabase()->CreatePlanner(penv, name);
1032 }
1033 
1035 {
1036  return RaveGlobal::instance()->GetDatabase()->CreateSensorSystem(penv, name);
1037 }
1038 
1040 {
1041  return RaveGlobal::instance()->GetDatabase()->CreateController(penv, name);
1042 }
1043 
1044 ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string& name)
1045 {
1046  return RaveGlobal::instance()->GetDatabase()->CreateModule(penv, name);
1047 }
1048 
1050 {
1051  return RaveGlobal::instance()->GetDatabase()->CreateModule(penv, name);
1052 }
1053 
1055 {
1056  return RaveGlobal::instance()->GetDatabase()->CreateModule(penv, name);
1057 }
1058 
1060 {
1061  return RaveGlobal::instance()->GetDatabase()->CreateIkSolver(penv, name);
1062 }
1063 
1065 {
1066  return RaveGlobal::instance()->GetDatabase()->CreatePhysicsEngine(penv, name);
1067 }
1068 
1069 SensorBasePtr RaveCreateSensor(EnvironmentBasePtr penv, const std::string& name)
1070 {
1071  return RaveGlobal::instance()->GetDatabase()->CreateSensor(penv, name);
1072 }
1073 
1075 {
1076  return RaveGlobal::instance()->GetDatabase()->CreateCollisionChecker(penv, name);
1077 }
1078 
1079 ViewerBasePtr RaveCreateViewer(EnvironmentBasePtr penv, const std::string& name)
1080 {
1081  return RaveGlobal::instance()->GetDatabase()->CreateViewer(penv, name);
1082 }
1083 
1084 KinBodyPtr RaveCreateKinBody(EnvironmentBasePtr penv, const std::string& name)
1085 {
1086  return RaveGlobal::instance()->GetDatabase()->CreateKinBody(penv, name);
1087 }
1088 
1090 {
1091  return RaveGlobal::instance()->GetDatabase()->CreateTrajectory(penv, name);
1092 }
1093 
1095 {
1096  return RaveCreateTrajectory(penv,"");
1097 }
1098 
1100 {
1101  return RaveGlobal::instance()->GetDatabase()->CreateSpaceSampler(penv, name);
1102 }
1103 
1104 UserDataPtr RaveRegisterInterface(InterfaceType type, const std::string& name, const char* interfacehash, const char* envhash, const boost::function<InterfaceBasePtr(EnvironmentBasePtr, std::istream&)>& createfn)
1105 {
1106  return RaveGlobal::instance()->GetDatabase()->RegisterInterface(type, name, interfacehash,envhash,createfn);
1107 }
1108 
1109 UserDataPtr RaveRegisterXMLReader(InterfaceType type, const std::string& xmltag, const CreateXMLReaderFn& fn)
1110 {
1111  return RaveGlobal::instance()->RegisterXMLReader(type,xmltag,fn);
1112 }
1113 
1114 BaseXMLReaderPtr RaveCallXMLReader(InterfaceType type, const std::string& xmltag, InterfaceBasePtr pinterface, const AttributesList& atts)
1115 {
1116  return RaveGlobal::instance()->CallXMLReader(type,xmltag,pinterface,atts);
1117 }
1118 
1119 std::string RaveFindLocalFile(const std::string& filename, const std::string& curdir)
1120 {
1121  return RaveGlobal::instance()->FindLocalFile(filename,curdir);
1122 }
1123 
1124 bool RaveInvertFileLookup(std::string& newfilename, const std::string& filename)
1125 {
1126  return RaveGlobal::instance()->InvertFileLookup(newfilename,filename);
1127 }
1128 
1129 void RaveSetDataAccess(int options)
1130 {
1131  RaveGlobal::instance()->SetDataAccess(options);
1132 }
1133 
1135 {
1136  return RaveGlobal::instance()->GetDataAccess();
1137 }
1138 
1140 {
1142  spec._vgroups.resize(1);
1143  spec._vgroups[0].offset = 0;
1144  spec._vgroups[0].dof = IkParameterization::GetNumberOfValues(iktype);
1145  spec._vgroups[0].name = str(boost::format("ikparam_values %d")%iktype);
1146  spec._vgroups[0].interpolation = interpolation;
1147  return spec;
1148 }
1149 
1150 std::ostream& operator<<(std::ostream& O, const IkParameterization &ikparam)
1151 {
1152  int type = ikparam._type;
1153  BOOST_ASSERT( !(type & IKP_CustomDataBit) );
1154  if( ikparam._mapCustomData.size() > 0 ) {
1155  type |= IKP_CustomDataBit;
1156  }
1157  O << type << " ";
1158  switch(ikparam._type & ~IKP_VelocityDataBit) {
1159  case IKP_Transform6D:
1160  O << ikparam.GetTransform6D();
1161  break;
1162  case IKP_Rotation3D:
1163  O << ikparam.GetRotation3D();
1164  break;
1165  case IKP_Translation3D: {
1166  Vector v = ikparam.GetTranslation3D();
1167  O << v.x << " " << v.y << " " << v.z << " ";
1168  break;
1169  }
1170  case IKP_Direction3D: {
1171  Vector v = ikparam.GetDirection3D();
1172  O << v.x << " " << v.y << " " << v.z << " ";
1173  break;
1174  }
1175  case IKP_Ray4D: {
1176  RAY r = ikparam.GetRay4D();
1177  O << r.dir.x << " " << r.dir.y << " " << r.dir.z << " " << r.pos.x << " " << r.pos.y << " " << r.pos.z << " ";
1178  break;
1179  }
1180  case IKP_Lookat3D: {
1181  Vector v = ikparam.GetLookat3D();
1182  O << v.x << " " << v.y << " " << v.z << " ";
1183  break;
1184  }
1186  RAY r = ikparam.GetTranslationDirection5D();
1187  O << r.dir.x << " " << r.dir.y << " " << r.dir.z << " " << r.pos.x << " " << r.pos.y << " " << r.pos.z << " ";
1188  break;
1189  }
1190  case IKP_TranslationXY2D: {
1191  Vector v = ikparam.GetTranslationXY2D();
1192  O << v.x << " " << v.y << " ";
1193  break;
1194  }
1196  Vector v = ikparam.GetTranslationXYOrientation3D();
1197  O << v.x << " " << v.y << " " << v.z << " ";
1198  break;
1199  }
1201  std::pair<Vector,Vector> p = ikparam.GetTranslationLocalGlobal6D();
1202  O << p.first.x << " " << p.first.y << " " << p.first.z << " " << p.second.x << " " << p.second.y << " " << p.second.z << " ";
1203  break;
1204  }
1206  std::pair<Vector,dReal> p = ikparam.GetTranslationXAxisAngle4D();
1207  O << p.second << " " << p.first.x << " " << p.first.y << " " << p.first.z << " ";
1208  break;
1209  }
1211  std::pair<Vector,dReal> p = ikparam.GetTranslationYAxisAngle4D();
1212  O << p.second << " " << p.first.x << " " << p.first.y << " " << p.first.z << " ";
1213  break;
1214  }
1216  std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngle4D();
1217  O << p.second << " " << p.first.x << " " << p.first.y << " " << p.first.z << " ";
1218  break;
1219  }
1221  std::pair<Vector,dReal> p = ikparam.GetTranslationXAxisAngleZNorm4D();
1222  O << p.second << " " << p.first.x << " " << p.first.y << " " << p.first.z << " ";
1223  break;
1224  }
1226  std::pair<Vector,dReal> p = ikparam.GetTranslationYAxisAngleXNorm4D();
1227  O << p.second << " " << p.first.x << " " << p.first.y << " " << p.first.z << " ";
1228  break;
1229  }
1231  std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngleYNorm4D();
1232  O << p.second << " " << p.first.x << " " << p.first.y << " " << p.first.z << " ";
1233  break;
1234  }
1235  default:
1236  throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterization 0x%x", ikparam.GetType(),ORE_InvalidArguments);
1237  }
1238  if( ikparam._mapCustomData.size() > 0 ) {
1239  O << ikparam._mapCustomData.size() << " ";
1240  FOREACHC(it, ikparam._mapCustomData) {
1241  O << it->first << " " << it->second.size() << " ";
1242  FOREACHC(itvalue, it->second) {
1243  O << *itvalue << " ";
1244  }
1245  }
1246  }
1247  return O;
1248 }
1249 
1250 std::istream& operator>>(std::istream& I, IkParameterization& ikparam)
1251 {
1252  int type=IKP_None;
1253  I >> type;
1254  ikparam._type = static_cast<IkParameterizationType>(type&~IKP_CustomDataBit);
1255  switch(ikparam._type) {
1256  case IKP_Transform6D: {
1257  Transform t; I >> t;
1258  ikparam.SetTransform6D(t);
1259  break;
1260  }
1262  I >> ikparam._transform;
1263  break;
1264  case IKP_Rotation3D: { Vector v; I >> v; ikparam.SetRotation3D(v); break; }
1266  I >> ikparam._transform.rot;
1267  break;
1268  case IKP_Translation3D: {
1269  Vector v;
1270  I >> v.x >> v.y >> v.z;
1271  ikparam.SetTranslation3D(v);
1272  break;
1273  }
1274  case IKP_Lookat3DVelocity:
1277  I >> ikparam._transform.trans.x >> ikparam._transform.trans.y >> ikparam._transform.trans.z;
1278  break;
1279  case IKP_Direction3D: { Vector v; I >> v.x >> v.y >> v.z; ikparam.SetDirection3D(v); break; }
1281  I >> ikparam._transform.rot.x >> ikparam._transform.rot.y >> ikparam._transform.rot.z;
1282  break;
1283  case IKP_Ray4D: { RAY r; I >> r.dir.x >> r.dir.y >> r.dir.z >> r.pos.x >> r.pos.y >> r.pos.z; ikparam.SetRay4D(r); break; }
1284  case IKP_Ray4DVelocity:
1286  I >> ikparam._transform.trans.x >> ikparam._transform.trans.y >> ikparam._transform.trans.z >> ikparam._transform.rot.x >> ikparam._transform.rot.y >> ikparam._transform.rot.z;
1287  break;
1288  case IKP_Lookat3D: { Vector v; I >> v.x >> v.y >> v.z; ikparam.SetLookat3D(v); break; }
1289  case IKP_TranslationDirection5D: { RAY r; I >> r.dir.x >> r.dir.y >> r.dir.z >> r.pos.x >> r.pos.y >> r.pos.z; ikparam.SetTranslationDirection5D(r); break; }
1290  case IKP_TranslationXY2D: { Vector v; I >> v.y >> v.y; ikparam.SetTranslationXY2D(v); break; }
1292  I >> ikparam._transform.trans.x >> ikparam._transform.trans.y;
1293  break;
1294  case IKP_TranslationXYOrientation3D: { Vector v; I >> v.y >> v.y >> v.z; ikparam.SetTranslationXYOrientation3D(v); break; }
1295  case IKP_TranslationLocalGlobal6D: { Vector localtrans, trans; I >> localtrans.x >> localtrans.y >> localtrans.z >> trans.x >> trans.y >> trans.z; ikparam.SetTranslationLocalGlobal6D(localtrans,trans); break; }
1297  I >> ikparam._transform.rot.x >> ikparam._transform.rot.y >> ikparam._transform.rot.z >> ikparam._transform.trans.x >> ikparam._transform.trans.y >> ikparam._transform.trans.z;
1298  break;
1300  Vector trans; dReal angle=0;
1301  I >> angle >> trans.x >> trans.y >> trans.z;
1302  ikparam.SetTranslationXAxisAngle4D(trans,angle);
1303  break;
1304  }
1306  Vector trans; dReal angle=0;
1307  I >> angle >> trans.x >> trans.y >> trans.z;
1308  ikparam.SetTranslationYAxisAngle4D(trans,angle);
1309  break;
1310  }
1312  Vector trans; dReal angle=0;
1313  I >> angle >> trans.x >> trans.y >> trans.z;
1314  ikparam.SetTranslationZAxisAngle4D(trans,angle);
1315  break;
1316  }
1318  Vector trans; dReal angle=0;
1319  I >> angle >> trans.x >> trans.y >> trans.z;
1320  ikparam.SetTranslationXAxisAngleZNorm4D(trans,angle);
1321  break;
1322  }
1324  Vector trans; dReal angle=0;
1325  I >> angle >> trans.x >> trans.y >> trans.z;
1326  ikparam.SetTranslationYAxisAngleXNorm4D(trans,angle);
1327  break;
1328  }
1330  Vector trans; dReal angle=0;
1331  I >> angle >> trans.x >> trans.y >> trans.z;
1332  ikparam.SetTranslationZAxisAngleYNorm4D(trans,angle);
1333  break;
1334  }
1341  I >> ikparam._transform.rot.x >> ikparam._transform.trans.x >> ikparam._transform.trans.y >> ikparam._transform.trans.z;
1342  break;
1343  default:
1344  throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterization 0x%x", ikparam.GetType(),ORE_InvalidArguments);
1345  }
1346  ikparam._mapCustomData.clear();
1347  if( type & IKP_CustomDataBit ) {
1348  size_t numcustom = 0, numvalues=0;
1349  std::string name;
1350  I >> numcustom;
1351  if( !I ) {
1352  return I;
1353  }
1354  for(size_t i = 0; i < numcustom; ++i) {
1355  I >> name >> numvalues;
1356  if( !I ) {
1357  return I;
1358  }
1359  std::vector<dReal>& v = ikparam._mapCustomData[name];
1360  v.resize(numvalues);
1361  for(size_t j = 0; j < v.size(); ++j) {
1362  I >> v[j];
1363  }
1364  }
1365  }
1366  return I;
1367 }
1368 
1369 int RaveGetIndexFromAffineDOF(int affinedofs, DOFAffine _dof)
1370 {
1371  int dof = static_cast<int>(_dof);
1372  dof &= affinedofs;
1373  int index = 0;
1374  if( dof&DOF_X ) {
1375  return index;
1376  }
1377  else if( affinedofs & DOF_X ) {
1378  ++index;
1379  }
1380  if( dof&DOF_Y ) {
1381  return index;
1382  }
1383  else if( affinedofs & DOF_Y ) {
1384  ++index;
1385  }
1386  if( dof&DOF_Z ) {
1387  return index;
1388  }
1389  else if( affinedofs & DOF_Z ) {
1390  ++index;
1391  }
1392  if( dof&DOF_RotationAxis ) {
1393  return index;
1394  }
1395  if( dof&DOF_Rotation3D ) {
1396  return index;
1397  }
1398  if( dof&DOF_RotationQuat ) {
1399  return index;
1400  }
1401  throw OPENRAVE_EXCEPTION_FORMAT("unspecified dof 0x%x, 0x%x",affinedofs%dof,ORE_InvalidArguments);
1402 }
1403 
1404 DOFAffine RaveGetAffineDOFFromIndex(int affinedofs, int requestedindex)
1405 {
1406  BOOST_ASSERT(requestedindex >= 0);
1407  int index = 0;
1408  if( index == requestedindex && (affinedofs&DOF_X) ) {
1409  return DOF_X;
1410  }
1411  else if( affinedofs & DOF_X ) {
1412  ++index;
1413  }
1414  if( index == requestedindex && (affinedofs&DOF_Y) ) {
1415  return DOF_Y;
1416  }
1417  else if( affinedofs & DOF_Y ) {
1418  ++index;
1419  }
1420  if( index == requestedindex && (affinedofs&DOF_Z)) {
1421  return DOF_Z;
1422  }
1423  else if( affinedofs & DOF_Z ) {
1424  ++index;
1425  }
1426  if( index <= requestedindex && index+3 > requestedindex && (affinedofs&DOF_RotationAxis) ) {
1427  return DOF_RotationAxis;
1428  }
1429  if( index <= requestedindex && index+3 > requestedindex && (affinedofs&DOF_Rotation3D) ) {
1430  return DOF_Rotation3D;
1431  }
1432  if( index <= requestedindex && index+4 > requestedindex && (affinedofs&DOF_RotationQuat) ) {
1433  return DOF_RotationQuat;
1434  }
1435  throw OPENRAVE_EXCEPTION_FORMAT("requested index out of bounds %d (affinemask=0x%x)",requestedindex%affinedofs, ORE_InvalidArguments);
1436 }
1437 
1438 int RaveGetAffineDOF(int affinedofs)
1439 {
1440  if( affinedofs & DOF_RotationAxis ) {
1441  BOOST_ASSERT( !(affinedofs & (DOF_Rotation3D|DOF_RotationQuat)) );
1442  }
1443  else if( affinedofs & DOF_Rotation3D ) {
1444  BOOST_ASSERT( !(affinedofs & (DOF_RotationAxis|DOF_RotationQuat)) );
1445  }
1446  else if( affinedofs & DOF_RotationQuat ) {
1447  BOOST_ASSERT( !(affinedofs & (DOF_Rotation3D|DOF_RotationAxis)) );
1448  }
1449  int dof = 0;
1450  if( affinedofs & DOF_X ) {
1451  dof++;
1452  }
1453  if( affinedofs & DOF_Y ) {
1454  dof++;
1455  }
1456  if( affinedofs & DOF_Z ) {
1457  dof++;
1458  }
1459  if( affinedofs & DOF_RotationAxis ) {
1460  dof++;
1461  }
1462  else if( affinedofs & DOF_Rotation3D ) {
1463  dof += 3;
1464  }
1465  else if( affinedofs & DOF_RotationQuat ) {
1466  dof += 4;
1467  }
1468  return dof;
1469 }
1470 
1471 void RaveGetAffineDOFValuesFromTransform(std::vector<dReal>::iterator itvalues, const Transform& t, int affinedofs, const Vector& vActvAffineRotationAxis)
1472 {
1473  if( affinedofs & DOF_X ) {
1474  *itvalues++ = t.trans.x;
1475  }
1476  if( affinedofs & DOF_Y ) {
1477  *itvalues++ = t.trans.y;
1478  }
1479  if( affinedofs & DOF_Z ) {
1480  *itvalues++ = t.trans.z;
1481  }
1482  if( affinedofs & DOF_RotationAxis ) {
1483  *itvalues++ = -normalizeAxisRotation(vActvAffineRotationAxis, t.rot).first;
1484  }
1485  else if( affinedofs & DOF_Rotation3D ) {
1486  dReal fsin = RaveSqrt(t.rot.y * t.rot.y + t.rot.z * t.rot.z + t.rot.w * t.rot.w);
1487  dReal fangle = 2 * atan2(fsin, t.rot.x);
1488  if( fsin > 0 ) {
1489  dReal normalizer = fangle / fsin;
1490  *itvalues++ = normalizer * t.rot.y;
1491  *itvalues++ = normalizer * t.rot.z;
1492  *itvalues++ = normalizer * t.rot.w;
1493  }
1494  else {
1495  *itvalues++ = 0;
1496  *itvalues++ = 0;
1497  *itvalues++ = 0;
1498  }
1499  }
1500  else if( affinedofs & DOF_RotationQuat ) {
1501  *itvalues++ = t.rot.x;
1502  *itvalues++ = t.rot.y;
1503  *itvalues++ = t.rot.z;
1504  *itvalues++ = t.rot.w;
1505  }
1506 }
1507 
1508 void RaveGetAffineDOFValuesFromVelocity(std::vector<dReal>::iterator itvalues, const Vector& linearvel, const Vector& angularvel, const Vector& quatrotation, int affinedofs, const Vector& axis)
1509 {
1510  if( affinedofs & DOF_X ) {
1511  *itvalues++ = linearvel.x;
1512  }
1513  if( affinedofs & DOF_Y ) {
1514  *itvalues++ = linearvel.y;
1515  }
1516  if( affinedofs & DOF_Z ) {
1517  *itvalues++ = linearvel.z;
1518  }
1519  if( affinedofs & DOF_RotationAxis ) {
1520  *itvalues++ = axis.dot3(angularvel);
1521  }
1522  else if( affinedofs & DOF_Rotation3D ) {
1523  *itvalues++ = angularvel.x;
1524  *itvalues++ = angularvel.y;
1525  *itvalues++ = angularvel.z;
1526  }
1527  else if( affinedofs & DOF_RotationQuat ) {
1528  Vector qdot = 0.5 * quatMultiply(Vector(0,angularvel.x,angularvel.y,angularvel.z), quatrotation);
1529  *itvalues++ = qdot.x;
1530  *itvalues++ = qdot.y;
1531  *itvalues++ = qdot.z;
1532  *itvalues++ = qdot.w;
1533  }
1534 }
1535 
1536 void RaveGetTransformFromAffineDOFValues(Transform& t, std::vector<dReal>::const_iterator itvalues, int affinedofs, const Vector& vActvAffineRotationAxis, bool normalize)
1537 {
1538  if( affinedofs & DOF_X ) {
1539  t.trans.x = *itvalues++;
1540  }
1541  if( affinedofs & DOF_Y ) {
1542  t.trans.y = *itvalues++;
1543  }
1544  if( affinedofs & DOF_Z ) {
1545  t.trans.z = *itvalues++;
1546  }
1547  if( affinedofs & DOF_RotationAxis ) {
1548  dReal angle = *itvalues++;
1549  dReal fsin = RaveSin(angle*(dReal)0.5);
1550  t.rot.x = RaveCos(angle*(dReal)0.5);
1551  t.rot.y = vActvAffineRotationAxis.x * fsin;
1552  t.rot.z = vActvAffineRotationAxis.y * fsin;
1553  t.rot.w = vActvAffineRotationAxis.z * fsin;
1554  }
1555  else if( affinedofs & DOF_Rotation3D ) {
1556  if( normalize ) {
1557  dReal x = *itvalues++;
1558  dReal y = *itvalues++;
1559  dReal z = *itvalues++;
1560  dReal fang = RaveSqrt(x*x + y*y + z*z);
1561  if( fang > 0 ) {
1562  dReal fnormalizer = sin((dReal)0.5 * fang) / fang;
1563  t.rot.x = cos((dReal)0.5 * fang);
1564  t.rot.y = fnormalizer * x;
1565  t.rot.z = fnormalizer * y;
1566  t.rot.w = fnormalizer * z;
1567  }
1568  else {
1569  t.rot = Vector(1,0,0,0); // identity
1570  }
1571  }
1572  else {
1573  // normalization turned off, but affine dofs have only DOF_Rotation3D. In order to convert this to quaternion velocity
1574  // will need current quaternion value (qrot), which is unknown, so assume identity
1575  // qvel = [0,axisangle] * qrot * 0.5
1576  t.rot[0] = 0;
1577  t.rot[1] = 0.5 * *itvalues++;
1578  t.rot[2] = 0.5 * *itvalues++;
1579  t.rot[3] = 0.5 * *itvalues++;
1580  }
1581  }
1582  else if( affinedofs & DOF_RotationQuat ) {
1583  // have to normalize since user might not be aware of this particular parameterization of rotations
1584  t.rot.x = *itvalues++;
1585  t.rot.y = *itvalues++;
1586  t.rot.z = *itvalues++;
1587  t.rot.w = *itvalues++;
1588  if( normalize ) {
1589  t.rot.normalize4();
1590  }
1591  }
1592 }
1593 
1594 void RaveGetVelocityFromAffineDOFVelocities(Vector& linearvel, Vector& angularvel, std::vector<dReal>::const_iterator itvalues, int affinedofs, const Vector& axis, const Vector& quatrotation)
1595 {
1596  if( affinedofs & DOF_X ) {
1597  linearvel.x = *itvalues++;
1598  }
1599  if( affinedofs & DOF_Y ) {
1600  linearvel.y = *itvalues++;
1601  }
1602  if( affinedofs & DOF_Z ) {
1603  linearvel.z = *itvalues++;
1604  }
1605  if( affinedofs & DOF_RotationAxis ) {
1606  dReal angle = *itvalues++;
1607  angularvel = axis*angle;
1608  }
1609  else if( affinedofs & DOF_Rotation3D ) {
1610  angularvel.x = *itvalues++;
1611  angularvel.y = *itvalues++;
1612  angularvel.z = *itvalues++;
1613  }
1614  else if( affinedofs & DOF_RotationQuat ) {
1615  // have to normalize since user might not be aware of this particular parameterization of rotations
1616  Vector q;
1617  q.x = *itvalues++;
1618  q.y = *itvalues++;
1619  q.z = *itvalues++;
1620  q.w = *itvalues++;
1621  Vector angularvel2 = quatMultiply(q, quatInverse(quatrotation)); // qdot = 0.5 * angularvel * q
1622  angularvel.x = angularvel2.y * 2;
1623  angularvel.y = angularvel2.z * 2;
1624  angularvel.z = angularvel2.w * 2;
1625  }
1626 }
1627 
1628 void CollisionReport::Reset(int coloptions)
1629 {
1630  options = coloptions;
1631  minDistance = 1e20f;
1632  numCols = 0;
1633  numWithinTol = 0;
1634  contacts.resize(0);
1635  vLinkColliding.resize(0);
1636 }
1637 
1638 std::string CollisionReport::__str__() const
1639 {
1640  stringstream s;
1641  s << "(";
1642  if( !!plink1 ) {
1643  s << plink1->GetParent()->GetName() << ":" << plink1->GetName();
1644  }
1645  s << ")x(";
1646  if( !!plink2 ) {
1647  s << plink2->GetParent()->GetName() << ":" << plink2->GetName();
1648  }
1649  s << ") contacts="<<contacts.size();
1650  return s.str();
1651 }
1652 
1654 {
1655  FOREACH(it, vertices) {
1656  *it = t * *it;
1657  }
1658 }
1659 
1661 {
1662  FOREACH(it, vertices) {
1663  *it = t * *it;
1664  }
1665 }
1666 
1667 void TriMesh::Append(const TriMesh& mesh)
1668 {
1669  int offset = (int)vertices.size();
1670  vertices.insert(vertices.end(), mesh.vertices.begin(), mesh.vertices.end());
1671  if( indices.capacity() < indices.size()+mesh.indices.size() ) {
1672  indices.reserve(indices.size()+mesh.indices.size());
1673  }
1674  FOREACHC(it, mesh.indices) {
1675  indices.push_back(*it+offset);
1676  }
1677 }
1678 
1679 void TriMesh::Append(const TriMesh& mesh, const Transform& trans)
1680 {
1681  int offset = (int)vertices.size();
1682  vertices.resize(vertices.size() + mesh.vertices.size());
1683  for(size_t i = 0; i < mesh.vertices.size(); ++i) {
1684  vertices[i+offset] = trans * mesh.vertices[i];
1685  }
1686  if( indices.capacity() < indices.size()+mesh.indices.size() ) {
1687  indices.reserve(indices.size()+mesh.indices.size());
1688  }
1689  FOREACHC(it, mesh.indices) {
1690  indices.push_back(*it+offset);
1691  }
1692 }
1693 
1695 {
1696  AABB ab;
1697  if( vertices.size() == 0 ) {
1698  return ab;
1699  }
1700  Vector vmin, vmax;
1701  vmin = vmax = vertices.at(0);
1702  FOREACHC(itv, vertices) {
1703  Vector v = *itv;
1704  if( vmin.x > v.x ) {
1705  vmin.x = v.x;
1706  }
1707  if( vmin.y > v.y ) {
1708  vmin.y = v.y;
1709  }
1710  if( vmin.z > v.z ) {
1711  vmin.z = v.z;
1712  }
1713  if( vmax.x < v.x ) {
1714  vmax.x = v.x;
1715  }
1716  if( vmax.y < v.y ) {
1717  vmax.y = v.y;
1718  }
1719  if( vmax.z < v.z ) {
1720  vmax.z = v.z;
1721  }
1722  }
1723 
1724  ab.extents = (dReal)0.5*(vmax-vmin);
1725  ab.pos = (dReal)0.5*(vmax+vmin);
1726  return ab;
1727 }
1728 
1729 void TriMesh::serialize(std::ostream& o, int options) const
1730 {
1731  o << vertices.size() << " ";
1732  FOREACHC(it,vertices) {
1733  o << it->x << " " << it->y << " " << it->z << " ";
1734  }
1735  o << indices.size() << " ";
1736  FOREACHC(it,indices) {
1737  o << *it << " ";
1738  }
1739 }
1740 
1741 std::ostream& operator<<(std::ostream& O, const TriMesh& trimesh)
1742 {
1743  trimesh.serialize(O,0);
1744  return O;
1745 }
1746 
1747 std::istream& operator>>(std::istream& I, TriMesh& trimesh)
1748 {
1749  trimesh.vertices.resize(0);
1750  trimesh.indices.resize(0);
1751  size_t s=0;
1752  I >> s;
1753  if( !I ) {
1754  return I;
1755  }
1756  trimesh.vertices.resize(s);
1757  FOREACH(it,trimesh.vertices) {
1758  I >> it->x >> it->y >> it->z;
1759  }
1760  I >> s;
1761  if( !I ) {
1762  return I;
1763  }
1764  trimesh.indices.resize(s);
1765  FOREACH(it,trimesh.indices) {
1766  I >> *it;
1767  }
1768  return I;
1769 }
1770 
1771 
1772 // Dummy Reader
1773 DummyXMLReader::DummyXMLReader(const std::string& fieldname, const std::string& pparentname, boost::shared_ptr<std::ostream> osrecord) : _fieldname(fieldname), _osrecord(osrecord)
1774 {
1775  _parentname = pparentname;
1776  _parentname += ":";
1777  _parentname += _fieldname;
1778 }
1779 
1781 {
1782  if( !!_pcurreader ) {
1783  if( _pcurreader->startElement(name, atts) == PE_Support ) {
1784  return PE_Support;
1785  }
1786  return PE_Ignore;
1787  }
1788 
1789  if( !!_osrecord ) {
1790  *_osrecord << "<" << name << " ";
1791  FOREACHC(itatt, atts) {
1792  *_osrecord << itatt->first << "=\"" << itatt->second << "\" ";
1793  }
1794  *_osrecord << ">" << endl;
1795  }
1796 
1797  // create a new parser
1798  _pcurreader.reset(new DummyXMLReader(name, _parentname,_osrecord));
1799  return PE_Support;
1800 }
1801 
1802 bool DummyXMLReader::endElement(const std::string& name)
1803 {
1804  if( !!_pcurreader ) {
1805  if( _pcurreader->endElement(name) ) {
1806  _pcurreader.reset();
1807  if( !!_osrecord ) {
1808  *_osrecord << "</" << name << ">" << endl;
1809  }
1810  }
1811  return false;
1812  }
1813 
1814  if( name == _fieldname ) {
1815  return true;
1816  }
1817  RAVELOG_ERROR(str(boost::format("invalid xml tag %s\n")%name));
1818  return false;
1819 }
1820 
1821 void DummyXMLReader::characters(const std::string& ch)
1822 {
1823  if( !_pcurreader ) {
1824  if( !!_osrecord ) {
1825  *_osrecord << ch;
1826  }
1827  }
1828  else {
1829  _pcurreader->characters(ch);
1830  }
1831 }
1832 
1834 {
1835  if( !RaveGlobalState() ) {
1836  RAVELOG_WARN("OpenRAVE global state not initialized! Need to call RaveInitialize before any OpenRAVE services can be used. For now, initializing with default parameters.\n");
1837  RaveInitialize(true);
1838  }
1839  __nUniqueId = RaveGlobal::instance()->RegisterEnvironment(this);
1840 }
1841 
1843 {
1844  RaveGlobal::instance()->UnregisterEnvironment(this);
1845 }
1846 
1847 
1848 bool SensorBase::SensorData::serialize(std::ostream& O) const
1849 {
1850  RAVELOG_WARN("SensorData XML serialization not implemented\n");
1851  return true;
1852 }
1853 
1854 bool SensorBase::LaserSensorData::serialize(std::ostream& O) const
1855 {
1856  RAVELOG_WARN("LaserSensorData XML serialization not implemented\n");
1857  return true;
1858 }
1859 
1860 bool SensorBase::CameraSensorData::serialize(std::ostream& O) const
1861 {
1862  RAVELOG_WARN("CameraSensorData XML serialization not implemented\n");
1863  return true;
1864 }
1865 
1867 {
1868  _oldoptions = p->GetCollisionOptions();
1869  _p = p;
1870  if( !_p->SetCollisionOptions(newoptions) ) {
1871  if( required ) {
1872  throw openrave_exception(str(boost::format("Failed to set collision options %d in checker %s\n")%newoptions%_p->GetXMLId()));
1873  }
1874  }
1875 }
1876 
1878 {
1879  _p->SetCollisionOptions(_oldoptions);
1880 }
1881 
1882 void RaveInitRandomGeneration(uint32_t seed)
1883 {
1884  RaveGlobal::instance()->GetDefaultSampler()->SetSeed(seed);
1885 }
1886 
1887 uint32_t RaveRandomInt()
1888 {
1889  std::vector<uint32_t> sample;
1890  RaveGlobal::instance()->GetDefaultSampler()->SampleSequence(sample);
1891  return sample.at(0);
1892 }
1893 
1895 {
1896  std::vector<dReal> sample;
1897  RaveGlobal::instance()->GetDefaultSampler()->SampleSequence(sample,1,interval);
1898  return float(sample.at(0));
1899 }
1900 
1902 {
1903  std::vector<dReal> sample;
1904  RaveGlobal::instance()->GetDefaultSampler()->SampleSequence(sample,1,interval);
1905  return double(sample.at(0));
1906 }
1907 
1908 namespace LocalXML {
1909 
1910 void RaveXMLErrorFunc(void *ctx, const char *msg, ...)
1911 {
1912  va_list args;
1913 
1914  va_start(args, msg);
1915  RAVELOG_ERROR("XML Parse error: ");
1916  vprintf(msg,args);
1917  va_end(args);
1918 }
1919 
1921 {
1922  XMLREADERDATA(BaseXMLReaderPtr preader, xmlParserCtxtPtr ctxt) : _preader(preader), _ctxt(ctxt) {
1923  }
1925  xmlParserCtxtPtr _ctxt;
1926 };
1927 
1928 void DefaultStartElementSAXFunc(void *ctx, const xmlChar *name, const xmlChar **atts)
1929 {
1930  AttributesList listatts;
1931  if( atts != NULL ) {
1932  for (int i = 0; (atts[i] != NULL); i+=2) {
1933  listatts.push_back(make_pair(string((const char*)atts[i]),string((const char*)atts[i+1])));
1934  std::transform(listatts.back().first.begin(), listatts.back().first.end(), listatts.back().first.begin(), ::tolower);
1935  }
1936  }
1937 
1938  XMLREADERDATA* pdata = (XMLREADERDATA*)ctx;
1939  string s = (const char*)name;
1940  std::transform(s.begin(), s.end(), s.begin(), ::tolower);
1941  if( !!pdata->_pdummy ) {
1942  RAVELOG_VERBOSE(str(boost::format("unknown field %s\n")%s));
1943  pdata->_pdummy->startElement(s,listatts);
1944  }
1945  else {
1946  BaseXMLReader::ProcessElement pestatus = pdata->_preader->startElement(s, listatts);
1947  if( pestatus != BaseXMLReader::PE_Support ) {
1948  // not handling, so create a temporary class to handle it
1949  pdata->_pdummy.reset(new DummyXMLReader(s,"(libxml)"));
1950  }
1951  }
1952 }
1953 
1954 void DefaultEndElementSAXFunc(void *ctx, const xmlChar *name)
1955 {
1956  XMLREADERDATA* pdata = (XMLREADERDATA*)ctx;
1957  string s = (const char*)name;
1958  std::transform(s.begin(), s.end(), s.begin(), ::tolower);
1959  if( !!pdata->_pdummy ) {
1960  if( pdata->_pdummy->endElement(s) ) {
1961  pdata->_pdummy.reset();
1962  }
1963  }
1964  else {
1965  if( pdata->_preader->endElement(s) ) {
1966  //RAVEPRINT(L"%s size read %d\n", name, data->_ctxt->input->consumed);
1967  xmlStopParser(pdata->_ctxt);
1968  }
1969  }
1970 }
1971 
1972 void DefaultCharactersSAXFunc(void *ctx, const xmlChar *ch, int len)
1973 {
1974  XMLREADERDATA* pdata = (XMLREADERDATA*)ctx;
1975  if( !!pdata->_pdummy ) {
1976  pdata->_pdummy->characters(string((const char*)ch, len));
1977  }
1978  else {
1979  pdata->_preader->characters(string((const char*)ch, len));
1980  }
1981 }
1982 
1983 bool xmlDetectSAX2(xmlParserCtxtPtr ctxt)
1984 {
1985  if (ctxt == NULL) {
1986  return false;
1987  }
1988 #ifdef LIBXML_SAX1_ENABLED
1989  if (ctxt->sax && ctxt->sax->initialized == XML_SAX2_MAGIC && (ctxt->sax->startElementNs != NULL || ctxt->sax->endElementNs != NULL)) {
1990  ctxt->sax2 = 1;
1991  }
1992 #else
1993  ctxt->sax2 = 1;
1994 #endif
1995 
1996  ctxt->str_xml = xmlDictLookup(ctxt->dict, BAD_CAST "xml", 3);
1997  ctxt->str_xmlns = xmlDictLookup(ctxt->dict, BAD_CAST "xmlns", 5);
1998  ctxt->str_xml_ns = xmlDictLookup(ctxt->dict, XML_XML_NAMESPACE, 36);
1999  if ( ctxt->str_xml==NULL || ctxt->str_xmlns==NULL || ctxt->str_xml_ns == NULL) {
2000  return false;
2001  }
2002  return true;
2003 }
2004 
2005 bool ParseXMLData(BaseXMLReaderPtr preader, const char* buffer, int size)
2006 {
2007  static xmlSAXHandler s_DefaultSAXHandler = { 0};
2008  if( size <= 0 ) {
2009  size = strlen(buffer);
2010  }
2011  if( !s_DefaultSAXHandler.initialized ) {
2012  // first time, so init
2013  s_DefaultSAXHandler.startElement = DefaultStartElementSAXFunc;
2014  s_DefaultSAXHandler.endElement = DefaultEndElementSAXFunc;
2015  s_DefaultSAXHandler.characters = DefaultCharactersSAXFunc;
2016  s_DefaultSAXHandler.error = RaveXMLErrorFunc;
2017  s_DefaultSAXHandler.initialized = 1;
2018  }
2019 
2020  xmlSAXHandlerPtr sax = &s_DefaultSAXHandler;
2021  int ret = 0;
2022  xmlParserCtxtPtr ctxt;
2023 
2024  ctxt = xmlCreateMemoryParserCtxt(buffer, size);
2025  if (ctxt == NULL) {
2026  return false;
2027  }
2028  if (ctxt->sax != (xmlSAXHandlerPtr) &xmlDefaultSAXHandler) {
2029  xmlFree(ctxt->sax);
2030  }
2031  ctxt->sax = sax;
2032  xmlDetectSAX2(ctxt);
2033 
2034  XMLREADERDATA reader(preader, ctxt);
2035  ctxt->userData = &reader;
2036 
2037  xmlParseDocument(ctxt);
2038 
2039  if (ctxt->wellFormed) {
2040  ret = 0;
2041  }
2042  else {
2043  if (ctxt->errNo != 0) {
2044  ret = ctxt->errNo;
2045  }
2046  else {
2047  ret = -1;
2048  }
2049  }
2050  if (sax != NULL) {
2051  ctxt->sax = NULL;
2052  }
2053  if (ctxt->myDoc != NULL) {
2054  xmlFreeDoc(ctxt->myDoc);
2055  ctxt->myDoc = NULL;
2056  }
2057  xmlFreeParserCtxt(ctxt);
2058 
2059  return ret==0;
2060 }
2061 
2062 }
2063 
2064 } // end namespace OpenRAVE