VrmlUtil.cpp
Go to the documentation of this file.
00001 
00005 #include "VrmlUtil.h"
00006 #include <cmath>
00007 
00008 using namespace std;
00009 
00010 namespace {
00011     void throwException(const std::string& fieldName, const std::string& expectedFieldType)
00012     {
00013         string error;
00014         error += "The node must have a field \"" + fieldName + "\" of " + expectedFieldType + " type";
00015         throw ModelLoader::ModelLoaderException(error.c_str());
00016     }
00017 }
00018 
00019 
00020 
00021 
00022 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, std::string& out_s)
00023 {
00024     VrmlVariantField& f = fmap[name];
00025     switch(f.typeId()){
00026     case SFSTRING:
00027         out_s = f.sfString();
00028         break;
00029     case MFSTRING:
00030     {
00031         MFString& strings = f.mfString();
00032         out_s = "";
00033         for(size_t i=0; i < strings.size(); i++){
00034             out_s += strings[i] + "\n";
00035         }
00036     }
00037     break;
00038     default:
00039         throwException(name, "SFString or MFString");
00040     }
00041 }
00042 
00043 
00044 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, DblSequence& out_v)
00045 {
00046     VrmlVariantField& f = fmap[name];
00047     switch(f.typeId()){
00048     case MFFLOAT:
00049     {
00050         MFFloat& mf = f.mfFloat();
00051         CORBA::ULong n = mf.size();
00052         out_v.length(n);
00053         for(CORBA::ULong i=0; i < n; ++i){
00054             out_v[i] = mf[i];
00055         }
00056     }
00057     break;
00058     default:
00059         throwException(name, "MFFloat");
00060     }
00061 }
00062 
00063 
00064 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, DblArray3& out_v)
00065 {
00066     VrmlVariantField& f = fmap[name];
00067     switch(f.typeId()){
00068     case SFVEC3F:
00069     case SFCOLOR:
00070     {
00071         SFVec3f& v = f.sfVec3f();
00072         for(int i = 0; i < 3; ++i){
00073             out_v[i] = v[i];
00074         }
00075     }
00076     break;
00077     default:
00078         throwException(name, "SFVec3f or SFColor");
00079     }
00080 }
00081     
00082 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, DblArray9& out_m)
00083 {
00084     VrmlVariantField& f = fmap[name];
00085     switch(f.typeId()){
00086     case MFFLOAT:
00087     {
00088         MFFloat& mf = f.mfFloat();
00089         if(mf.size() == 9){
00090             for(int i=0; i < 9; ++i){
00091                 out_m[i] = mf[i];
00092             }
00093         } else {
00094             throw ModelLoader::ModelLoaderException("illegal size of a matrix field");
00095         }
00096     }
00097     break;
00098     default:
00099         throwException(name, "MFFloat");
00100     }
00101 }
00102 
00103 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, CORBA::Double& out_v)
00104 {
00105     VrmlVariantField& f = fmap[name];
00106     switch(f.typeId()){
00107     case SFFLOAT:
00108         out_v = f.sfFloat();
00109         break;
00110     default:
00111         throwException(name, "SFFloat");
00112     }
00113 }
00114 
00115 
00116 void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, CORBA::Long& out_v)
00117 {
00118     VrmlVariantField& f = fmap[name];
00119     switch(f.typeId()){
00120     case SFINT32:
00121         out_v = f.sfInt32();
00122         break;
00123     default:
00124         throwException(name, "SFInt32");
00125     }
00126 }
00127 
00128 
00129 
00130 //void copyVrmlField(TProtoFieldMap& fmap, const std::string& name, CORBA::Long& out_v)
00131 //{
00132 //      VrmlVariantField& f = fmap[name];
00133 //      switch( f.typeId() )
00134 //      {
00135 //      case SFINT32:
00136 //              out_v = f.sfInt32();
00137 //              break;
00138 //      default:
00139 //              throwException( name, "SFInt32" );
00140 //      }
00141 //}
00142 
00143 
00144 
00145 void copyVrmlRotationFieldToDblArray9
00146 (TProtoFieldMap& fieldMap, const std::string name, DblArray9& out_R)
00147 {
00148     VrmlVariantField& rotationField = fieldMap[name];
00149 
00150     if(rotationField.typeId() != SFROTATION){
00151         throwException(name, "SFRotation");
00152     }
00153 
00154     SFRotation& r = rotationField.sfRotation();
00155 
00156     const double& theta = r[3];
00157     const double sth = sin(theta);
00158     const double vth = 1.0 - cos(theta);
00159 
00160     double ax = r[0];
00161     double ay = r[1];
00162     double az = r[2];
00163 
00164     // normalization
00165     double l = sqrt(ax*ax + ay*ay + az*az);
00166         
00167     //if(fabs(l - 1.0) > 1.0e-6){
00168     //    throwException(name, "Not normalized axis");
00169     //}
00170            
00171     ax /= l;
00172     ay /= l;
00173     az /= l;
00174 
00175     const double axx = ax*ax*vth;
00176     const double ayy = ay*ay*vth;
00177     const double azz = az*az*vth;
00178     const double axy = ax*ay*vth;
00179     const double ayz = ay*az*vth;
00180     const double azx = az*ax*vth;
00181 
00182     ax *= sth;
00183     ay *= sth;
00184     az *= sth;
00185 
00186     out_R[0] = 1.0 - azz - ayy; out_R[1] = -az + axy;       out_R[2] = ay + azx;
00187     out_R[3] = az + axy;        out_R[4] = 1.0 - azz - axx; out_R[5] = -ax + ayz;
00188     out_R[6] = -ay + azx;       out_R[7] = ax + ayz;        out_R[8] = 1.0 - ayy - axx;
00189 }
00190 
00191 
00192 void copyVrmlRotationFieldToDblArray4
00193 (TProtoFieldMap& fieldMap, const std::string name, DblArray4& out_R)
00194 {
00195     VrmlVariantField& rotationField = fieldMap[name];
00196 
00197     if(rotationField.typeId() != SFROTATION)
00198         {
00199             throwException( name, "SFRotation" );
00200         }
00201 
00202     SFRotation& r = rotationField.sfRotation();
00203 
00204     for( int i = 0 ; i < 4 ; i++ )
00205         {
00206             out_R[i] = r[i];
00207         }
00208 }
00209 
00210 string setTexturefileUrl(string modelfileDir, MFString urls){
00211     string retUrl("");
00212     //  ImageTextureに格納されている MFString url の数を確認 //
00213     if( 0 == urls.size() )
00214     {
00215         string error;
00216         error += "ImageTexture read error: No urls in ImageTexture node";
00217         throw ModelLoader::ModelLoaderException(error.c_str());
00218     }else{
00219         for(unsigned int i=0; i<urls.size(); i++){
00220                 getPathFromUrl( retUrl, modelfileDir, urls[i] );
00221                 if( !retUrl.empty() )
00222                         break;
00223         }
00224     }
00225     return retUrl;
00226 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:57