VrmlUtil.cpp
Go to the documentation of this file.
1 
5 #include "VrmlUtil.h"
6 #include <cmath>
7 
8 using namespace std;
9 
10 namespace {
11  void throwException(const std::string& fieldName, const std::string& expectedFieldType)
12  {
13  string error;
14  error += "The node must have a field \"" + fieldName + "\" of " + expectedFieldType + " type";
15  throw ModelLoader::ModelLoaderException(error.c_str());
16  }
17 }
18 
19 
20 
21 
22 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, std::string& out_s)
23 {
24  VrmlVariantField& f = fmap[name];
25  switch(f.typeId()){
26  case SFSTRING:
27  out_s = f.sfString();
28  break;
29  case MFSTRING:
30  {
31  MFString& strings = f.mfString();
32  out_s = "";
33  for(size_t i=0; i < strings.size(); i++){
34  out_s += strings[i] + "\n";
35  }
36  }
37  break;
38  default:
39  throwException(name, "SFString or MFString");
40  }
41 }
42 
43 
44 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, DblSequence& out_v)
45 {
46  VrmlVariantField& f = fmap[name];
47  switch(f.typeId()){
48  case MFFLOAT:
49  {
50  MFFloat& mf = f.mfFloat();
51  CORBA::ULong n = mf.size();
52  out_v.length(n);
53  for(CORBA::ULong i=0; i < n; ++i){
54  out_v[i] = mf[i];
55  }
56  }
57  break;
58  default:
59  throwException(name, "MFFloat");
60  }
61 }
62 
63 
64 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, DblArray3& out_v)
65 {
66  VrmlVariantField& f = fmap[name];
67  switch(f.typeId()){
68  case SFVEC3F:
69  case SFCOLOR:
70  {
71  SFVec3f& v = f.sfVec3f();
72  for(int i = 0; i < 3; ++i){
73  out_v[i] = v[i];
74  }
75  }
76  break;
77  default:
78  throwException(name, "SFVec3f or SFColor");
79  }
80 }
81 
82 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, DblArray9& out_m)
83 {
84  VrmlVariantField& f = fmap[name];
85  switch(f.typeId()){
86  case MFFLOAT:
87  {
88  MFFloat& mf = f.mfFloat();
89  if(mf.size() == 9){
90  for(int i=0; i < 9; ++i){
91  out_m[i] = mf[i];
92  }
93  } else {
94  throw ModelLoader::ModelLoaderException("illegal size of a matrix field");
95  }
96  }
97  break;
98  default:
99  throwException(name, "MFFloat");
100  }
101 }
102 
103 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, CORBA::Double& out_v)
104 {
105  VrmlVariantField& f = fmap[name];
106  switch(f.typeId()){
107  case SFFLOAT:
108  out_v = f.sfFloat();
109  break;
110  default:
111  throwException(name, "SFFloat");
112  }
113 }
114 
115 
116 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, CORBA::Long& out_v)
117 {
118  VrmlVariantField& f = fmap[name];
119  switch(f.typeId()){
120  case SFINT32:
121  out_v = f.sfInt32();
122  break;
123  default:
124  throwException(name, "SFInt32");
125  }
126 }
127 
128 
129 
130 //void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, CORBA::Long& out_v)
131 //{
132 // VrmlVariantField& f = fmap[name];
133 // switch( f.typeId() )
134 // {
135 // case SFINT32:
136 // out_v = f.sfInt32();
137 // break;
138 // default:
139 // throwException( name, "SFInt32" );
140 // }
141 //}
142 
143 
144 
146 (TProtoFieldMap& fieldMap, const std::string name, DblArray9& out_R)
147 {
148  VrmlVariantField& rotationField = fieldMap[name];
149 
150  if(rotationField.typeId() != SFROTATION){
151  throwException(name, "SFRotation");
152  }
153 
154  SFRotation& r = rotationField.sfRotation();
155 
156  const double& theta = r[3];
157  const double sth = sin(theta);
158  const double vth = 1.0 - cos(theta);
159 
160  double ax = r[0];
161  double ay = r[1];
162  double az = r[2];
163 
164  // normalization
165  double l = sqrt(ax*ax + ay*ay + az*az);
166 
167  //if(fabs(l - 1.0) > 1.0e-6){
168  // throwException(name, "Not normalized axis");
169  //}
170 
171  ax /= l;
172  ay /= l;
173  az /= l;
174 
175  const double axx = ax*ax*vth;
176  const double ayy = ay*ay*vth;
177  const double azz = az*az*vth;
178  const double axy = ax*ay*vth;
179  const double ayz = ay*az*vth;
180  const double azx = az*ax*vth;
181 
182  ax *= sth;
183  ay *= sth;
184  az *= sth;
185 
186  out_R[0] = 1.0 - azz - ayy; out_R[1] = -az + axy; out_R[2] = ay + azx;
187  out_R[3] = az + axy; out_R[4] = 1.0 - azz - axx; out_R[5] = -ax + ayz;
188  out_R[6] = -ay + azx; out_R[7] = ax + ayz; out_R[8] = 1.0 - ayy - axx;
189 }
190 
191 
193 (TProtoFieldMap& fieldMap, const std::string name, DblArray4& out_R)
194 {
195  VrmlVariantField& rotationField = fieldMap[name];
196 
197  if(rotationField.typeId() != SFROTATION)
198  {
199  throwException( name, "SFRotation" );
200  }
201 
202  SFRotation& r = rotationField.sfRotation();
203 
204  for( int i = 0 ; i < 4 ; i++ )
205  {
206  out_R[i] = r[i];
207  }
208 }
209 
210 string setTexturefileUrl(string modelfileDir, MFString urls){
211  string retUrl("");
212  // ImageTextureに格納されている MFString url の数を確認 //
213  if( 0 == urls.size() )
214  {
215  string error;
216  error += "ImageTexture read error: No urls in ImageTexture node";
217  throw ModelLoader::ModelLoaderException(error.c_str());
218  }else{
219  for(unsigned int i=0; i<urls.size(); i++){
220  getPathFromUrl( retUrl, modelfileDir, urls[i] );
221  if( !retUrl.empty() )
222  break;
223  }
224  }
225  return retUrl;
226 }
copyVrmlRotationFieldToDblArray9
void copyVrmlRotationFieldToDblArray9(TProtoFieldMap &fieldMap, const std::string name, DblArray9 &out_R)
Definition: VrmlUtil.cpp:146
hrp::SFROTATION
@ SFROTATION
Definition: VrmlNodes.h:40
i
png_uint_32 i
Definition: png.h:2732
setTexturefileUrl
string setTexturefileUrl(string modelfileDir, MFString urls)
Definition: VrmlUtil.cpp:210
hrp::VrmlVariantField
Definition: VrmlNodes.h:796
hrp::SFVEC3F
@ SFVEC3F
Definition: VrmlNodes.h:39
hrp::SFVec3f
boost::array< SFFloat, 3 > SFVec3f
Definition: VrmlNodes.h:55
hrp::TProtoFieldMap
std::map< std::string, VrmlVariantField > TProtoFieldMap
Definition: VrmlNodes.h:854
hrp::MFSTRING
@ MFSTRING
Definition: VrmlNodes.h:43
autoplay.n
n
Definition: autoplay.py:12
swingTest.f
f
Definition: swingTest.py:6
hrp::MFString
std::vector< SFString > MFString
Definition: VrmlNodes.h:77
hrp::SFFLOAT
@ SFFLOAT
Definition: VrmlNodes.h:37
copyVrmlField
void copyVrmlField(TProtoFieldMap &fmap, const std::string &name, std::string &out_s)
Definition: VrmlUtil.cpp:22
VrmlUtil.h
hrp::getPathFromUrl
HRP_UTIL_EXPORT void getPathFromUrl(string &refUrl, const string &rootDir, string srcUrl)
Definition: UrlUtil.cpp:57
hrp::SFRotation
SFVec4f SFRotation
Definition: VrmlNodes.h:59
name
png_infop png_charpp name
Definition: png.h:2379
hrp::MFFloat
std::vector< SFFloat > MFFloat
Definition: VrmlNodes.h:70
copyVrmlRotationFieldToDblArray4
void copyVrmlRotationFieldToDblArray4(TProtoFieldMap &fieldMap, const std::string name, DblArray4 &out_R)
Definition: VrmlUtil.cpp:193
hrp::SFINT32
@ SFINT32
Definition: VrmlNodes.h:36
hrp::SFCOLOR
@ SFCOLOR
Definition: VrmlNodes.h:42
error
void error(char *msg) const
Definition: minigzip.c:87
hrp::VrmlVariantField::sfRotation
SFRotation sfRotation
Definition: VrmlNodes.h:805
hrp::SFSTRING
@ SFSTRING
Definition: VrmlNodes.h:43
hrp::MFFLOAT
@ MFFLOAT
Definition: VrmlNodes.h:37
hrp::VrmlVariantField::typeId
VrmlFieldTypeId typeId()
Definition: VrmlNodes.h:828


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04