kin model loading improved (for unreal kitchen)

rai/Core/array.cpp

@@ -926,6 +926,17 @@ void remove_alpha_channel(byteA &img) {
   img.reshape(h, w, 3);
 }
 
+void image_halfResolution(byteA &img){
+  byteA org = img;
+  img.resize(org.d0/2, org.d1/2, org.d2);
+  for(uint i=0;i<img.d0;i++) for(uint j=0;j<img.d1;j++) for(uint k=0;k<img.d2;k++){
+    float v = (float)org(2*i, 2*j, k) + (float)org(2*i, 2*j+1, k)
+            + (float)org(2*i+1, 2*j, k) +(float)org(2*i+1, 2*j+1, k);
+    v /= 4;
+    img(i,j,k) = (byte)v;
+  }
+}
+
 void flip_image(byteA &img) {
   if(!img.N) return;
   uint h=img.d0, n=img.N/img.d0;
@@ -2291,6 +2302,3 @@ void linkArray() { cout <<"*** libArray.so dynamically loaded ***" <<endl; }
 //}
 
 
-
-
-

rai/Core/array.h

@@ -638,6 +638,7 @@ void make_RGB2BGRA(byteA &img);
 void swap_RGB_BGR(byteA &img);
 void flip_image(byteA &img);
 void flip_image(floatA &img);
+void image_halfResolution(byteA& img);
 
 void scanArrFile(const char* name);
 

rai/Core/graph.cpp

@@ -682,7 +682,7 @@ Node* Graph::readNode(std::istream& is, bool verbose, bool parseInfo, rai::Strin
         }
       } break;
       case '<': { //any type parser
-#if 0
+#if 1
         str.read(is, "", ">", true);
         node = newNode<rai::String>(keys, parents, str);
 #else

rai/Geo/geo.cpp

@@ -1296,12 +1296,11 @@ void Transformation::read(std::istream& is) {
   for(;;) {
     is >>c;
     if(is.fail()) return;  //EOF I guess
-    //if(c==';') break;
-    //if(c==',') is >>c;
     if((c>='0' && c<='9') || c=='.' || c=='-') {  //read a 7-vector (pos+quat) for the transformation
       is.putback(c);
       is>>x[0]>>x[1]>>x[2];       addRelativeTranslation(x[0], x[1], x[2]);
       is>>x[0]>>x[1]>>x[2]>>x[3]; addRelativeRotationQuat(x[0], x[1], x[2], x[3]);
+      break;
     } else switch(c) {
           //case '<': break; //do nothing -- assume this is an opening tag
         case 't': is>>PARSE("(")>>x[0]>>x[1]>>x[2]>>PARSE(")");       addRelativeTranslation(x[0], x[1], x[2]); break;

rai/Geo/geoms.cpp

@@ -50,6 +50,8 @@ void rai::Geom::read(const Graph &ats){
   if(ats.get(d, "meshscale"))  { mesh.scale(d); }
   if(ats.get(x, "meshscale"))  { mesh.scale(x(0), x(1), x(2)); }
 
+  if(mesh.V.N && type==ST_none) type=ST_mesh;
+
   createMeshes();
 
   //colored box?
@@ -99,8 +101,9 @@ void rai::Geom::createMeshes(){
   case rai::ST_mesh:
   case rai::ST_pointCloud:
     CHECK(mesh.V.N, "mesh needs to be loaded");
-    sscCore = mesh;
-    sscCore.makeConvexHull();
+    size(3) = 0.;
+//    sscCore = mesh;
+//    sscCore.makeConvexHull();
     break;
   case rai::ST_ssCvx:
     CHECK(size(3)>1e-10,"");

rai/Geo/mesh.cpp

@@ -1007,6 +1007,7 @@ void rai::Mesh::read(std::istream& is, const char* fileExtension, const char* fi
   if(!strcmp(fileExtension, "off")) { readOffFile(is); loaded=true; }
   if(!strcmp(fileExtension, "ply")) { readPLY(filename); loaded=true; }
   if(!strcmp(fileExtension, "tri")) { readTriFile(is); loaded=true; }
+  if(!strcmp(fileExtension, "arr")) { readArr(is); loaded=true; }
   if(!strcmp(fileExtension, "stl") || !strcmp(fileExtension, "STL")) { loaded = readStlFile(is); }
   if(!strcmp(fileExtension, "dae") || !strcmp(fileExtension, "DAE")) { *this = mesh_readAssimp(filename); }
   if(!loaded) HALT("can't read fileExtension '" <<fileExtension <<"' file '" <<filename <<"'");
@@ -1175,7 +1176,7 @@ void rai::Mesh::readPLY(const char *fn) {
   };
 
   FILE    *fp  = fopen(fn, "r");
-  if(!fp) return;
+  CHECK(fp, "coult not open file " <<fn)
   PlyFile *ply = read_ply(fp);
 
   //-- get the number of faces and vertices
@@ -1236,11 +1237,29 @@ void rai::Mesh::readPLY(const char *fn) {
   close_ply(ply); //calls fclose
   free_ply(ply);
 }
+
 #else
 void rai::Mesh::writePLY(const char *fn, bool bin) { NICO }
 void rai::Mesh::readPLY(const char *fn) { NICO }
 #endif
 
+void rai::Mesh::writeArr(std::ostream& os){
+  V.writeTagged(os, "V", true);
+  T.writeTagged(os, "T", true);
+  C.writeTagged(os, "C", true);
+  tex.writeTagged(os, "tex", true);
+  texImg.writeTagged(os, "texImg", true);
+}
+
+void rai::Mesh::readArr(std::istream& is){
+  V.readTagged(is, "V");
+  T.readTagged(is, "T");
+  C.readTagged(is, "C");
+  tex.readTagged(is, "tex");
+  texImg.readTagged(is, "texImg");
+}
+
+
 bool rai::Mesh::readStlFile(std::istream& is) {
   //first check if binary
   if(rai::parse(is, "solid", true)) { //is ascii
@@ -1636,30 +1655,52 @@ void rai::Mesh::glDraw(struct OpenGL& gl) {
   //-- draw a mesh
   if(V.d0!=Vn.d0 || T.d0!=Tn.d0) computeNormals();
 
-#if 0
-  if(!C.N || C.nd==1 || C.d0==V.d0){ //we have colors for each vertex -> use index arrays
+  //-- if not yet done, GenTexture
+  if(texImg.N && texture<0){
+    GLuint texName;
+    glGenTextures(1, &texName);
+    texture = texName;
+    glBindTexture(GL_TEXTURE_2D, texture);
+
+    if(texImg.d2==4) glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, texImg.d1, texImg.d0, 0, GL_RGBA, GL_UNSIGNED_BYTE, texImg.p);
+    else if(texImg.d2==3) glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texImg.d1, texImg.d0, 0, GL_RGB, GL_UNSIGNED_BYTE, texImg.p);
+    else NIY;
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+  }else{
+    glBindTexture(GL_TEXTURE_2D, texture);
+  }
 
-    if(Vt.N) glBindTexture(GL_TEXTURE_2D, texture);
 
+#if 1
+  if(!C.N || C.nd==1 || C.d0==V.d0){ //we have colors for each vertex -> use index arrays
+
+    if(tex.N) CHECK_EQ(tex.d0, V.d0, "this needs tex coords for each vertex; if you have it face wise, render the slow way..")
+    if(tex.N) glEnable(GL_TEXTURE_2D);
 
     //  glShadeModel(GL_FLAT);
     glShadeModel(GL_SMOOTH);
     glEnableClientState(GL_VERTEX_ARRAY);
     glEnableClientState(GL_NORMAL_ARRAY);
     if(C.N==V.N) glEnableClientState(GL_COLOR_ARRAY); else glDisableClientState(GL_COLOR_ARRAY);
     if(C.N==V.N) glDisable(GL_LIGHTING); //because lighting requires ambiance colors to be set..., not just color..
-    if(Vt.N) glEnableClientState(GL_TEXTURE_COORD_ARRAY); else glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+    if(tex.N) glEnableClientState(GL_TEXTURE_COORD_ARRAY); else glDisableClientState(GL_TEXTURE_COORD_ARRAY);
 
 
     glVertexPointer(3, GL_DOUBLE, 0, V.p);
     glNormalPointer(GL_DOUBLE, 0, Vn.p);
     if(C.N==V.N) glColorPointer(3, GL_DOUBLE, 0, C.p);
-    if(Vt.N) glTexCoordPointer(2, GL_DOUBLE, 0, Vt.p );
+    if(tex.N) glTexCoordPointer(2, GL_DOUBLE, 0, tex.p );
 
     glDrawElements(GL_TRIANGLES, T.N, GL_UNSIGNED_INT, T.p);
 
     if(C.N) glEnable(GL_LIGHTING);
 
+    if(tex.N) glDisable(GL_TEXTURE_2D);
+
+
   }else{ //we have colors for each tri -> render tris directly and with tri-normals
 
     CHECK_EQ(C.d0, T.d0, "");
@@ -1686,28 +1727,10 @@ void rai::Mesh::glDraw(struct OpenGL& gl) {
   }
 #elif 1 //simple with vertex normals
   uint i, v;
-  if(Tt.N && texImg.N && Geo_mesh_drawColors){
-    glShadeModel(GL_SMOOTH);
+  if(Tt.N && Geo_mesh_drawColors){
+//    glShadeModel(GL_SMOOTH);
     glEnable(GL_TEXTURE_2D);
-
-    if(texture<0){
-      GLuint texName;
-      glGenTextures(1, &texName);
-      texture = texName;
-      glBindTexture(GL_TEXTURE_2D, texture);
-
-      if(texImg.d2==4) glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, texImg.d1, texImg.d0, 0, GL_RGBA, GL_UNSIGNED_BYTE, texImg.p);
-      else if(texImg.d2==3) glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texImg.d1, texImg.d0, 0, GL_RGB, GL_UNSIGNED_BYTE, texImg.p);
-      else NIY;
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
-    }else{
-      glBindTexture(GL_TEXTURE_2D, texture);
-    }
-    //    glColor3f(1.,1.,1.);
-    glDisable(GL_LIGHTING);
+//    glDisable(GL_LIGHTING);
   }
   glBegin(GL_TRIANGLES);
   for(i=0; i<T.d0; i++) {

rai/Geo/mesh.h

@@ -119,6 +119,9 @@ struct Mesh : GLDrawer {
   void writeOffFile(const char* filename);
   void writePLY(const char *fn, bool bin);
   void readPLY(const char *fn);
+  void writeArr(std::ostream&);
+  void readArr(std::istream&);
+
   void glDraw(struct OpenGL&);
 };
 } //END of namespace

rai/Geo/mesh_readAssimp.cpp

@@ -7,6 +7,8 @@
 #include <assimp/scene.h>
 #include <assimp/postprocess.h>
 
+bool loadTextures = true;
+
 struct AssimpLoader {
   std::vector<rai::Mesh> meshes;
   std::string directory;
@@ -60,12 +62,12 @@ struct AssimpLoader {
     rai::Mesh M;
     M.V.resize(mesh->mNumVertices, 3);
     M.Vn.resize(mesh->mNumVertices, 3);
-    if(mesh->mTextureCoords[0]) M.tex.resize(mesh->mNumVertices, 2);
+    if(loadTextures && mesh->mTextureCoords[0]) M.tex.resize(mesh->mNumVertices, 2);
 
     for(unsigned int i = 0; i < mesh->mNumVertices; i++) {
       M.V[i] = ARR( mesh->mVertices[i].x, mesh->mVertices[i].y, mesh->mVertices[i].z );
       M.Vn[i] = ARR(mesh->mNormals[i].x, mesh->mNormals[i].y, mesh->mNormals[i].z );
-      if(mesh->mTextureCoords[0]) M.tex[i] = ARR(mesh->mTextureCoords[0][i].x, mesh->mTextureCoords[0][i].y);
+      if(loadTextures && mesh->mTextureCoords[0]) M.tex[i] = ARR(mesh->mTextureCoords[0][i].x, mesh->mTextureCoords[0][i].y);
     }
 
     M.T.resize(mesh->mNumFaces, 3);
@@ -78,7 +80,7 @@ struct AssimpLoader {
     }
 //cout <<"mean of loaded mesh=" <<M.getCenter() <<endl;
 
-    if(mesh->mTextureCoords[0]) M.Tt = M.T;
+    if(loadTextures && mesh->mTextureCoords[0]) M.Tt = M.T;
 
     aiMaterial* material = scene->mMaterials[mesh->mMaterialIndex];
 
@@ -94,7 +96,7 @@ struct AssimpLoader {
 
     uint nTex = material->GetTextureCount(aiTextureType_DIFFUSE);
 //    cout <<"material: #textures=" <<nTex <<endl;
-    if(nTex){
+    if(loadTextures && nTex){
       CHECK_EQ(nTex, 1, "");
       aiString str;
       material->GetTexture(aiTextureType_DIFFUSE, 0, &str);

rai/Kin/frame.cpp

@@ -118,7 +118,7 @@ void rai::Frame::read(const Graph& ats) {
   if(ats["type"]) ats["type"]->keys.last() = "shape"; //compatibility with old convention: 'body { type... }' generates shape
 
   if(ats["joint"]){ joint = new Joint(*this); joint->read(ats); }
-  if(ats["shape"]){ shape = new Shape(*this); shape->read(ats); }
+  if(ats["shape"] || ats["mesh"]){ shape = new Shape(*this); shape->read(ats); }
   if(ats["mass"]){ inertia = new Inertia(*this); inertia->read(ats); }
 }
 
@@ -132,14 +132,14 @@ void rai::Frame::write(std::ostream& os) const {
   if(inertia) inertia->write(os);
 
   if(parent){
-    if(!Q.isZero()) os <<" Q=<T " <<Q <<" > ";
+    if(!Q.isZero()) os <<" Q:<" <<Q <<'>';
   }else{
-    if(!X.isZero()) os <<" X=<T " <<X <<" > ";
+    if(!X.isZero()) os <<" X:<" <<X <<'>';
   }
 
   if(flags){
     Enum<FrameFlagType> fl;
-    os <<" FLAGS=";
+    os <<" FLAGS:";
     for(int i=0;;i++){
       fl.x = FrameFlagType(i);
       if(!fl.name()) break;
@@ -153,8 +153,8 @@ void rai::Frame::write(std::ostream& os) const {
   }
 
   os <<" }\n";
-  //  if(mass) os <<"mass=" <<mass <<' ';
-  //  if(type!=BT_dynamic) os <<"dyntype=" <<(int)type <<' ';
+  //  if(mass) os <<"mass:" <<mass <<' ';
+  //  if(type!=BT_dynamic) os <<"dyntype:" <<(int)type <<' ';
   //  uint i; Node *a;
   //  for(Type *  a:  ats)
   //      if(a->keys(0)!="X" && a->keys(0)!="pose") os <<*a <<' ';
@@ -655,11 +655,11 @@ void rai::Joint::read(const Graph &G){
 }
 
 void rai::Joint::write(std::ostream& os) const {
-  os <<" joint=" <<type;
-  if(H) os <<" ctrl_H="<<H;
+  os <<" joint:" <<type;
+  if(H) os <<" ctrl_H:"<<H;
   if(limits.N) os <<" limits=[" <<limits <<"]";
   if(mimic){
-    os <<" mimic=" <<mimic->frame.name;
+    os <<" mimic:" <<mimic->frame.name;
   }
 
   Node *n;
@@ -679,7 +679,7 @@ rai::Shape::Shape(Frame &f, const Shape *copyShape)
   frame.shape = this;
   if(copyShape){
     const Shape& s = *copyShape;
-    mesh_radius=s.mesh_radius;
+//    mesh_radius=s.mesh_radius;
     cont=s.cont;
     geom = s.geom;
   }
@@ -694,6 +694,12 @@ rai::Geom &rai::Shape::getGeom(){
   return *geom;
 }
 
+void rai::Shape::setGeomMimic(const rai::Frame *f){
+  CHECK(!geom, "");
+  CHECK(f->shape->geom, "");
+  geom = f->shape->geom;
+}
+
 
 void rai::Shape::read(const Graph& ats) {
 
@@ -713,15 +719,17 @@ void rai::Shape::read(const Graph& ats) {
   }
 
   //compute the bounding radius
-  if(mesh().V.N) mesh_radius = mesh().getRadius();
+//  if(mesh().V.N) mesh_radius = mesh().getRadius();
 }
 
 void rai::Shape::write(std::ostream& os) const {
   if(geom){
-    os <<" shape=" <<geom->type;
-    os <<" size=[" <<geom->size <<"]";
+    os <<" shape:" <<geom->type;
+    if(geom->type!=ST_mesh)
+      os <<" size:[" <<geom->size <<"]";
   }else{
-    os <<" shape=NONE";
+    HALT("you shouldn't be here");
+    os <<" shape:NONE";
   }
 
   Node *n;
@@ -817,7 +825,7 @@ arr rai::Inertia::getFrameRelativeWrench(){
 }
 
 void rai::Inertia::write(std::ostream &os) const{
-  os <<" mass=" <<mass;
+  os <<" mass:" <<mass;
 }
 
 void rai::Inertia::read(const Graph& G){

rai/Kin/frame.h

@@ -170,6 +170,7 @@ struct Shape : GLDrawer{
   Geom *geom = NULL;
 
   Geom& getGeom(); ///< creates a geom if not yet initialized
+  void setGeomMimic( const Frame* f );
   Enum<ShapeType>& type() { return getGeom().type; }
   arr& size() { return getGeom().size; }
   double& size(uint i) { return getGeom().size.elem(i); }
@@ -180,7 +181,7 @@ struct Shape : GLDrawer{
 //  Enum<ShapeType> type;
 //  arr size;
 //  Mesh mesh, sscCore;
-  double mesh_radius=0.;
+//  double mesh_radius=0.;
   bool cont=false;           ///< are contacts registered (or filtered in the callback)
 
   Shape(Frame& f, const Shape *copyShape=NULL); //new Shape, being added to graph and body's shape lists