[Jderobot-admin] jderobot-r927 - in trunk/src/libs: . geometry geometry/collada geometry/math

ahcorde en jderobot.org ahcorde en jderobot.org
Mie Jun 5 12:27:13 CEST 2013


Author: ahcorde
Date: 2013-06-05 12:26:11 +0200 (Wed, 05 Jun 2013)
New Revision: 927

Added:
   trunk/src/libs/geometry/
   trunk/src/libs/geometry/collada/
   trunk/src/libs/geometry/collada/colladaparser.cpp
   trunk/src/libs/geometry/collada/colladaparser.h
   trunk/src/libs/geometry/collada/color.cpp
   trunk/src/libs/geometry/collada/color.h
   trunk/src/libs/geometry/collada/malla.cpp
   trunk/src/libs/geometry/collada/malla.h
   trunk/src/libs/geometry/collada/material.cpp
   trunk/src/libs/geometry/collada/material.h
   trunk/src/libs/geometry/collada/submalla.cpp
   trunk/src/libs/geometry/collada/submalla.h
   trunk/src/libs/geometry/math/
   trunk/src/libs/geometry/math/matriz3x3.cpp
   trunk/src/libs/geometry/math/matriz3x3.h
   trunk/src/libs/geometry/math/matriz4x4.cpp
   trunk/src/libs/geometry/math/matriz4x4.h
   trunk/src/libs/geometry/math/utils.cpp
   trunk/src/libs/geometry/math/utils.h
   trunk/src/libs/geometry/math/vector2d.cpp
   trunk/src/libs/geometry/math/vector2d.h
   trunk/src/libs/geometry/math/vector3.cpp
   trunk/src/libs/geometry/math/vector3.h
Log:
[ahcorde] Libreria geometry 
		- Parseador de collada
		- Libreria matem?\195?\161tica 


Added: trunk/src/libs/geometry/collada/colladaparser.cpp
===================================================================
--- trunk/src/libs/geometry/collada/colladaparser.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/collada/colladaparser.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,1336 @@
+#include "colladaparser.h"
+
+ColladaParser::ColladaParser(std::string filename)
+{
+    TiXmlDocument xmlDoc;
+
+    this->filename = filename;
+
+    std::cout << "Loading " << filename << std::endl;
+
+    if (!xmlDoc.LoadFile(this->filename)){
+        std::cout << "Imposible to load collada file" << std::endl;
+        return;
+    }
+    this->colladaXml = xmlDoc.FirstChildElement("COLLADA");
+    if (!this->colladaXml){
+        std::cout << "Falta la etiqueta Collada" << std::endl;
+        return;
+    }
+    if (std::string(this->colladaXml->Attribute("version")) != "1.4.0" &&
+        std::string(this->colladaXml->Attribute("version")) != "1.4.1"){
+        std::cout << "Invalid version. Must be version 1.4.0 or 1.4.1" << std::endl;
+        return;
+
+    }
+    TiXmlElement *assetXml = this->colladaXml->FirstChildElement("asset");
+    if (assetXml)
+    {
+      TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
+      if (unitXml && unitXml->Attribute("meter")){
+            this->meter = math::parseFloat(unitXml->Attribute("meter"));
+            std::cout << "meter: " << this->meter << std::endl;
+      }
+    }
+    mesh = new Malla();
+    mesh->setPath(this->filename);
+
+    loadScene(mesh);
+}
+
+int ite = 0;
+
+void ColladaParser::draw()
+{
+
+    if(ite==0){
+        ite++;
+        std::vector<std::string> listaNombres;
+        std::vector<unsigned int> listaTexturas;
+        for(int i = 0; i < this->mesh->getSubMeshCount();i++){
+            SubMalla* submalla = this->mesh->getSubMesh(i);
+
+            int indiceMaterial = submalla->getMaterialIndex();
+
+            if(indiceMaterial==-1){
+                continue;
+            }
+
+            Material* material = this->mesh->getMaterial(indiceMaterial);
+
+            std::string name = material->texImage;
+            bool encontrado =false;
+            int indiceEncontrado = 0;
+            for(int j = 0; j < listaNombres.size(); j++){
+                if( listaNombres[j].compare(name)==0 ){
+                    encontrado = true;
+                    indiceEncontrado =j;
+                    break;
+                }
+            }
+
+            if(!encontrado){
+                listaNombres.push_back(name);
+
+                unsigned int m_nID;
+                glGenTextures(1, &m_nID);
+                glBindTexture(GL_TEXTURE_2D, m_nID);
+
+                cv::Mat image = material->getImage();
+
+                gluBuild2DMipmaps( GL_TEXTURE_2D, 3,
+                               image.cols,
+                               image.rows,
+                               GL_BGR, GL_UNSIGNED_BYTE,
+                               image.data );
+                listaTexturas.push_back(m_nID);
+                material->idTextura = m_nID;
+            }else{
+                material->idTextura = listaTexturas[indiceEncontrado];
+            }
+        }
+    }
+
+    for(int i = 0; i < this->mesh->getSubMeshCount();i++){
+
+        SubMalla* submalla = this->mesh->getSubMesh(i);
+
+        int indiceMaterial = submalla->getMaterialIndex();
+        Material* material = this->mesh->getMaterial(indiceMaterial);
+
+        if(submalla->getTexCoordCount()>0){
+            glEnable(GL_TEXTURE_2D);
+            glBindTexture(GL_TEXTURE_2D, material->idTextura);
+        }
+
+
+        Color c;
+        if(indiceMaterial>0){
+            c = material->getDiffuse();
+        }else{
+            c.r = 0.7;
+            c.g = 0.7;
+            c.b = 0.7;
+            c.a = 1;
+        }
+
+
+
+        if(submalla->getTexCoordCount()==0){
+            glColor3f(c.r, c.g, c.b);
+
+        }
+
+        glPointSize(5.0);
+        glBegin(GL_POLYGON);
+        for(int k = 0; k < submalla->getVertexCount(); k++){
+
+            if(k%3==0)
+                glBegin(GL_POLYGON);
+
+            unsigned int indice =submalla->getIndex(k);
+
+            math::Vector3 v= submalla->getVertex(indice);
+            math::Vector3 normals= submalla->getNormal(indice);
+            glNormal3f(normals.vector(0), normals.vector(1), normals.vector(2));
+
+            if(submalla->getTexCoordCount()>0){
+                math::Vector2d text= submalla->getTexCoord(indice);
+                glTexCoord2f(text.vector(0), text.vector(1));
+
+            }
+            glVertex3f(v.vector(0), v.vector(1), v.vector(2));
+
+            if(k%3==2)
+                glEnd();
+
+        }
+
+        glEnd();
+
+        if(submalla->getTexCoordCount()>0){
+            glDisable(GL_TEXTURE_2D);
+        }
+
+    }
+}
+
+
+/////////////////////////////////////////////////
+void ColladaParser::loadScene(Malla *_mesh)
+{
+  TiXmlElement *sceneXml = this->colladaXml->FirstChildElement("scene");
+  std::string sceneURL =
+    sceneXml->FirstChildElement("instance_visual_scene")->Attribute("url");
+
+  TiXmlElement *visSceneXml = this->getElementId("visual_scene", sceneURL);
+
+  if (!visSceneXml)
+  {
+    std::cout << "Unable to find visual_scene id ='" << sceneURL << "'\n";
+    return;
+  }
+
+  TiXmlElement *nodeXml = visSceneXml->FirstChildElement("node");
+  while (nodeXml)
+  {
+    this->loadNode(nodeXml, _mesh, math::Matriz4x4::IDENTITY);
+    nodeXml = nodeXml->NextSiblingElement("node");
+  }
+}
+
+/////////////////////////////////////////////////
+TiXmlElement *ColladaParser::getElementId(const std::string &_name,
+                                          const std::string &_id)
+{
+  return this->getElementId(this->colladaXml, _name, _id);
+}
+
+/////////////////////////////////////////////////
+TiXmlElement *ColladaParser::getElementId(TiXmlElement *_parent,
+    const std::string &_name,
+    const std::string &_id)
+{
+  std::string id = _id;
+  if (id.length() > 0 && id[0] == '#')
+    id.erase(0, 1);
+
+  if ((id.empty() && _parent->Value() == _name) ||
+      (_parent->Attribute("id") && _parent->Attribute("id") == id) ||
+      (_parent->Attribute("sid") && _parent->Attribute("sid") == id))
+  {
+    return _parent;
+  }
+
+  TiXmlElement *elem = _parent->FirstChildElement();
+  while (elem)
+  {
+    TiXmlElement *result = this->getElementId(elem, _name, _id);
+    if (result)
+    {
+      return result;
+    }
+
+    elem = elem->NextSiblingElement();
+  }
+
+  return NULL;
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadNode(TiXmlElement *_elem, Malla *_mesh,
+    const math::Matriz4x4 &_transform)
+{
+  TiXmlElement *nodeXml;
+  TiXmlElement *instGeomXml;
+
+  math::Matriz4x4 transform = this->loadNodeTransform(_elem);
+  transform = _transform * transform;
+
+  if (_elem->Attribute("name"))
+  {
+    this->currentNodeName = _elem->Attribute("name");
+  }
+
+  nodeXml = _elem->FirstChildElement("node");
+  while (nodeXml)
+  {
+    this->loadNode(nodeXml, _mesh, transform);
+    nodeXml = nodeXml->NextSiblingElement("node");
+  }
+
+  if (_elem->FirstChildElement("instance_node"))
+  {
+    std::string nodeURLStr =
+      _elem->FirstChildElement("instance_node")->Attribute("url");
+
+    nodeXml = this->getElementId("node", nodeURLStr);
+    if (!nodeXml)
+    {
+      std::cout << "Unable to find node[" << nodeURLStr << "]\n";
+      return;
+    }
+    this->loadNode(nodeXml, _mesh, transform);
+    return;
+  }
+  else
+    nodeXml = _elem;
+
+  instGeomXml = nodeXml->FirstChildElement("instance_geometry");
+  while (instGeomXml)
+  {
+    std::string geomURL = instGeomXml->Attribute("url");
+    TiXmlElement *geomXml = this->getElementId("geometry", geomURL);
+
+    this->materialMap.clear();
+    TiXmlElement *bindMatXml, *techniqueXml, *matXml;
+    bindMatXml = instGeomXml->FirstChildElement("bind_material");
+    while (bindMatXml)
+    {
+      if ((techniqueXml = bindMatXml->FirstChildElement("technique_common")))
+      {
+        matXml = techniqueXml->FirstChildElement("instance_material");
+        while (matXml)
+        {
+          std::string symbol = matXml->Attribute("symbol");
+          std::string target = matXml->Attribute("target");
+          this->materialMap[symbol] = target;
+          matXml = matXml->NextSiblingElement("instance_material");
+        }
+      }
+      bindMatXml = bindMatXml->NextSiblingElement("bind_material");
+    }
+
+    this->loadGeometry(geomXml, transform, _mesh);
+    instGeomXml = instGeomXml->NextSiblingElement("instance_geometry");
+  }
+
+  TiXmlElement *instContrXml =
+    nodeXml->FirstChildElement("instance_controller");
+  while (instContrXml)
+  {
+    std::string contrURL = instContrXml->Attribute("url");
+    TiXmlElement *contrXml = this->getElementId("controller", contrURL);
+
+    TiXmlElement *instSkelXml = instContrXml->FirstChildElement("skeleton");
+    std::string rootURL = instSkelXml->GetText();
+    TiXmlElement *rootNodeXml = this->getElementId("node", rootURL);
+
+    this->materialMap.clear();
+    TiXmlElement *bindMatXml, *techniqueXml, *matXml;
+    bindMatXml = instContrXml->FirstChildElement("bind_material");
+    while (bindMatXml)
+    {
+      if ((techniqueXml = bindMatXml->FirstChildElement("technique_common")))
+      {
+        matXml = techniqueXml->FirstChildElement("instance_material");
+        while (matXml)
+        {
+          std::string symbol = matXml->Attribute("symbol");
+          std::string target = matXml->Attribute("target");
+          this->materialMap[symbol] = target;
+          matXml = matXml->NextSiblingElement("instance_material");
+        }
+      }
+      bindMatXml = bindMatXml->NextSiblingElement("bind_material");
+    }
+
+    this->loadController(contrXml, rootNodeXml, transform, _mesh);
+    instContrXml = instContrXml->NextSiblingElement("instance_controller");
+  }
+}
+
+/////////////////////////////////////////////////
+math::Matriz4x4 ColladaParser::loadNodeTransform(TiXmlElement *_elem)
+{
+  math::Matriz4x4 transform(math::Matriz4x4::IDENTITY);
+
+  if (_elem->FirstChildElement("matrix"))
+  {
+    std::string matrixStr = _elem->FirstChildElement("matrix")->GetText();
+    std::istringstream iss(matrixStr);
+    std::vector<double> values(16);
+    for (unsigned int i = 0; i < 16; i++)
+      iss >> values[i];
+
+    transform.set(values[0], values[1], values[2], values[3],
+                  values[4], values[5], values[6], values[7],
+                  values[8], values[9], values[10], values[11],
+                  values[12], values[13], values[14], values[15]);
+  }
+  else
+  {
+
+    if (_elem->FirstChildElement("scale"))
+    {
+        std::string scaleStr = _elem->FirstChildElement("scale")->GetText();
+        math::Vector3 scale;
+        scale = boost::lexical_cast<math::Vector3>(scaleStr);
+        math::Matriz4x4 scaleMat;
+        scaleMat.setScale(scale);
+        transform = transform * scaleMat;
+    }
+
+    if (_elem->FirstChildElement("translate"))
+    {
+      std::string transStr = _elem->FirstChildElement("translate")->GetText();
+      math::Vector3 translate;
+      translate = boost::lexical_cast<math::Vector3>(transStr);
+      // translate *= this->meter;
+      transform.setTranslate(translate);
+    }
+
+    TiXmlElement *rotateXml = _elem->FirstChildElement("rotate");
+
+    while (rotateXml)
+    {
+      math::Matriz3x3 mat;
+      math::Vector3 axis;
+      double angle;
+
+      std::string rotateStr = rotateXml->GetText();
+      std::istringstream iss(rotateStr);
+
+      float x, y, z;
+
+      iss >> x >> y >> z;
+
+      axis.setX(x);
+      axis.setY(y);
+      axis.setZ(z);
+
+      iss >> angle;
+      mat.setFromAxis(axis.getX(), axis.getY(), axis.getZ(), angle*3.1416/180);
+      transform = transform * mat;
+
+      rotateXml = rotateXml->NextSiblingElement("rotate");
+    }
+
+
+  }
+
+  return transform;
+}
+
+
+/////////////////////////////////////////////////
+void ColladaParser::loadVertices(const std::string &_id,
+                const math::Matriz4x4 &_transform,
+                std::vector<math::Vector3> &_verts,
+                std::vector<math::Vector3> &_norms)
+{
+  TiXmlElement *verticesXml = this->getElementId(this->colladaXml,
+                                                 "vertices", _id);
+
+  if (!verticesXml)
+  {
+    std::cout << "Unable to find vertices[" << _id << "] in collada file\n";
+    return;
+  }
+
+  TiXmlElement *inputXml = verticesXml->FirstChildElement("input");
+  while (inputXml)
+  {
+    std::string semantic = inputXml->Attribute("semantic");
+    std::string sourceStr = inputXml->Attribute("source");
+    if (semantic == "NORMAL")
+    {
+      this->loadNormals(sourceStr, _transform, _norms);
+    }
+    else if (semantic == "POSITION")
+    {
+      this->loadPositions(sourceStr, _transform, _verts);
+    }
+
+    inputXml = inputXml->NextSiblingElement("input");
+  }
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadPositions(const std::string &_id,
+                    const math::Matriz4x4 &_transform,
+                    std::vector<math::Vector3> &_values)
+{
+  TiXmlElement *sourceXml = this->getElementId("source", _id);
+  TiXmlElement *floatArrayXml = sourceXml->FirstChildElement("float_array");
+  if (!floatArrayXml)
+  {
+    std::cout  << "Vertex source missing float_array element\n";
+    return;
+  }
+  std::string valueStr = floatArrayXml->GetText();
+
+  std::vector<std::string> strs;
+  std::vector<std::string>::iterator iter, end;
+  boost::split(strs, valueStr, boost::is_any_of("   "));
+
+  end = strs.end();
+  for (iter = strs.begin(); iter != end; iter += 3)
+  {
+    math::Vector3 vec(math::parseFloat(*iter), math::parseFloat(*(iter+1)),
+        math::parseFloat(*(iter+2)));
+    vec = _transform * vec;
+    _values.push_back(vec);
+  }
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadNormals(const std::string &_id,
+                                const math::Matriz4x4 &_transform,
+                                std::vector<math::Vector3> &_values)
+{
+  TiXmlElement *normalsXml = this->getElementId("source", _id);
+  if (!normalsXml)
+  {
+    std::cout  << "Unable to find normals[" << _id << "] in collada file\n";
+    return;
+  }
+
+  TiXmlElement *floatArrayXml = normalsXml->FirstChildElement("float_array");
+  if (!floatArrayXml)
+  {
+    std::cout  << "Normal source missing float_array element\n";
+    return;
+  }
+
+  std::string valueStr = floatArrayXml->GetText();
+  std::istringstream iss(valueStr);
+  do
+  {
+    math::Vector3 vec;
+    float x, y, z;
+    iss >> x >> y >> z;
+    vec.setX(x);
+    vec.setY(y);
+    vec.setZ(z);
+
+    if (iss)
+    {
+      vec = _transform * vec;
+      vec.normalize();
+      _values.push_back(vec);
+    }
+  } while (iss);
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadGeometry(TiXmlElement *_xml,
+                                 const math::Matriz4x4 &_transform, Malla *_mesh)
+{
+  TiXmlElement *meshXml = _xml->FirstChildElement("mesh");
+  TiXmlElement *childXml;
+
+  if (!meshXml)
+    return;
+
+  childXml = meshXml->FirstChildElement("triangles");
+  while (childXml)
+  {
+    this->loadTriangles(childXml, _transform, _mesh);
+    childXml = childXml->NextSiblingElement("triangles");
+  }
+
+  childXml = meshXml->FirstChildElement("polylist");
+  while (childXml)
+  {
+    this->loadPolylist(childXml, _transform, _mesh);
+    childXml = childXml->NextSiblingElement("polylist");
+  }
+
+  childXml = meshXml->FirstChildElement("lines");
+  while (childXml)
+  {
+    this->loadLines(childXml, _transform, _mesh);
+    childXml = childXml->NextSiblingElement("lines");
+  }
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadLines(TiXmlElement *_xml,
+    const math::Matriz4x4 &_transform,
+    Malla *_mesh)
+{
+  SubMalla *subMalla = new SubMalla();
+  subMalla->setName(this->currentNodeName);
+  subMalla->setPrimitiveType(SubMalla::LINES);
+
+  TiXmlElement *inputXml = _xml->FirstChildElement("input");
+  // std::string semantic = inputXml->Attribute("semantic");
+  std::string source = inputXml->Attribute("source");
+
+  std::vector<math::Vector3> verts;
+  std::vector<math::Vector3> norms;
+  this->loadVertices(source, _transform, verts, norms);
+
+  TiXmlElement *pXml = _xml->FirstChildElement("p");
+  std::string pStr = pXml->GetText();
+  std::istringstream iss(pStr);
+
+  do
+  {
+    int a, b;
+    iss >> a >> b;
+
+    if (!iss)
+      break;
+    subMalla->addVertex(verts[a]);
+    subMalla->addIndex(subMalla->getVertexCount() - 1);
+    subMalla->addVertex(verts[b]);
+    subMalla->addIndex(subMalla->getVertexCount() - 1);
+  } while (iss);
+
+  _mesh->addSubMalla(subMalla);
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadPolylist(TiXmlElement *_polylistXml,
+    const math::Matriz4x4 &_transform,
+    Malla *_mesh)
+{
+  // This function parses polylist types in collada into
+  // a set of triangle meshes.  The assumption is that
+  // each polylist polygon is convex, and we do decomposion
+  // by anchoring each triangle about vertex 0 or each polygon
+  SubMalla *subMalla = new SubMalla;
+  subMalla->setName(this->currentNodeName);
+  bool combinedVertNorms = false;
+
+  subMalla->setPrimitiveType(SubMalla::TRIANGLES);
+
+  if (_polylistXml->Attribute("material"))
+  {
+    std::map<std::string, std::string>::iterator iter;
+    std::string matStr = _polylistXml->Attribute("material");
+
+    iter = this->materialMap.find(matStr);
+    if (iter != this->materialMap.end())
+      matStr = iter->second;
+
+    int matIndex = _mesh->addMaterial(this->loadMaterial(matStr));
+    if (matIndex < 0)
+      std::cout << "Unable to add material[" << matStr << "]\n";
+    else
+      subMalla->setMaterialIndex(matIndex);
+  }
+
+  TiXmlElement *polylistInputXml = _polylistXml->FirstChildElement("input");
+
+  std::vector<math::Vector3> verts;
+  std::vector<math::Vector3> norms;
+  std::vector<math::Vector2d> texcoords;
+
+  math::Matriz4x4 bindShapeMat(math::Matriz4x4::IDENTITY);
+  //if (_mesh->HasSkeleton())
+  //  bindShapeMat = _mesh->GetSkeleton()->GetBindShapeTransform();
+
+  // read input elements
+  std::map<std::string, int> inputs;
+  while (polylistInputXml)
+  {
+    std::string semantic = polylistInputXml->Attribute("semantic");
+    std::string source = polylistInputXml->Attribute("source");
+    std::string offset = polylistInputXml->Attribute("offset");
+    if (semantic == "VERTEX")
+    {
+      unsigned int count = norms.size();
+      this->loadVertices(source, _transform, verts, norms);
+      if (norms.size() > count)
+        combinedVertNorms = true;
+    }
+    else if (semantic == "NORMAL")
+    {
+      this->loadNormals(source, _transform, norms);
+      combinedVertNorms = false;
+    }
+    else if (semantic == "TEXCOORD")
+      this->loadTexCoords(source, texcoords);
+
+    inputs[semantic] = math::parseInt(offset);
+
+    polylistInputXml = polylistInputXml->NextSiblingElement("input");
+  }
+
+  // read vcount
+  // break poly into triangles
+  // if vcount >= 4, anchor around 0 (note this is bad for concave elements)
+  //   e.g. if vcount = 4, break into triangle 1: [0,1,2], triangle 2: [0,2,3]
+  std::vector<std::string> vcountStrs;
+  TiXmlElement *vcountXml = _polylistXml->FirstChildElement("vcount");
+  std::string vcountStr = vcountXml->GetText();
+  boost::split(vcountStrs, vcountStr, boost::is_any_of("   "));
+  std::vector<int> vcounts;
+  for (unsigned int j = 0; j < vcountStrs.size(); ++j)
+    vcounts.push_back(math::parseInt(vcountStrs[j]));
+
+  // read p
+  TiXmlElement *pXml = _polylistXml->FirstChildElement("p");
+  std::string pStr = pXml->GetText();
+
+  std::vector<math::Vector3> vertNorms(verts.size());
+  std::vector<int> vertNormsCounts(verts.size());
+  std::fill(vertNormsCounts.begin(), vertNormsCounts.end(), 0);
+
+  int *values = new int[inputs.size()];
+  std::map<std::string, int>::iterator end = inputs.end();
+  std::map<std::string, int>::iterator iter;
+  math::Vector2d vec;
+
+  std::vector<std::string> strs;
+  boost::split(strs, pStr, boost::is_any_of("   "));
+  std::vector<std::string>::iterator strs_iter = strs.begin();
+  for (unsigned int l = 0; l < vcounts.size(); ++l)
+  {
+    // put us at the beginning of the polygon list
+    if (l > 0) strs_iter += inputs.size()*vcounts[l-1];
+
+    for (unsigned int k = 2; k < (unsigned int)vcounts[l]; ++k)
+    {
+      // if vcounts[l] = 5, then read 0,1,2, then 0,2,3, 0,3,4,...
+      // here k = the last number in the series
+      // j is the triangle loop
+      for (unsigned int j = 0; j < 3; ++j)
+      {
+        // break polygon into triangles
+        unsigned int triangle_index;
+
+        if (j == 0)
+          triangle_index = 0;
+        if (j == 1)
+          triangle_index = (k-1)*inputs.size();
+        if (j == 2)
+          triangle_index = (k)*inputs.size();
+
+        for (unsigned int i = 0; i < inputs.size(); i++)
+        {
+          values[i] = math::parseInt(strs_iter[triangle_index+i]);
+//          std::cout << "debug parsing "
+//                << " poly-i[" << l
+//                << "] tri-end-index[" << k
+//                << "] tri-vertex-i[" << j
+//                << "] triangle[" << triangle_index
+//                << "] input[" << i
+//                << "] value[" << values[i]
+//                << "]\n";
+        }
+
+
+        for (iter = inputs.begin(); iter != end; ++iter)
+        {
+          if (iter->first == "VERTEX")
+          {
+            subMalla->addVertex(bindShapeMat * verts[values[iter->second]]);
+            subMalla->addIndex(subMalla->getVertexCount()-1);
+            if (combinedVertNorms)
+              subMalla->addNormal(norms[values[iter->second]]);
+//            if (_mesh->HasSkeleton())
+//            {
+//              Skeleton *skel = _mesh->GetSkeleton();
+//              for (unsigned int i = 0;
+//                  i < skel->GetNumVertNodeWeights(values[iter->second]); i++)
+//              {
+//                std::pair<std::string, double> node_weight =
+//                              skel->GetVertNodeWeight(values[iter->second], i);
+//                SkeletonNode *node =
+//                  _mesh->GetSkeleton()->GetNodeByName(node_weight.first);
+//                SubMalla->AddNodeAssignment(SubMalla->GetVertexCount()-1,
+//                            node->GetHandle(), node_weight.second);
+//              }
+//            }
+          }
+          else if (iter->first == "NORMAL")
+          {
+            subMalla->addNormal(norms[values[iter->second]]);
+          }
+          else if (iter->first == "TEXCOORD")
+          {
+              subMalla->addTexCoord(texcoords[values[iter->second]].getX(),
+                                    texcoords[values[iter->second]].getY());
+          }
+          // else
+          // gzerr << "Unhandled semantic[" << iter->first << "]\n";
+        }
+      }
+    }
+  }
+  delete [] values;
+
+  _mesh->addSubMalla(subMalla);
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadTexCoords(const std::string &_id,
+                                  std::vector<math::Vector2d> &_values)
+{
+  int stride = 0;
+  int texCount = 0;
+  int totCount = 0;
+
+  // Get the source element for the texture coordinates.
+  TiXmlElement *xml = this->getElementId("source", _id);
+  if (!xml)
+  {
+    std::cout << "Unable to find tex coords[" << _id << "] in collada file\n";
+    return;
+  }
+
+  // Get the array of float values. These are the raw values for the texture
+  // coordinates.
+  TiXmlElement *floatArrayXml = xml->FirstChildElement("float_array");
+  if (!floatArrayXml)
+  {
+    std::cout << "Normal source missing float_array element\n";
+    return;
+  }
+
+  // The technique_common holds an <accessor> element that indicates how to
+  // parse the float array.
+  xml = xml->FirstChildElement("technique_common");
+  if (!xml)
+  {
+    std::cout << "Unable to find technique_common element for texture "
+          << "coordinates with id[" << _id << "]\n";
+    return;
+  }
+
+  // Get the accessor XML element.
+  xml = xml->FirstChildElement("accessor");
+  if (!xml)
+  {
+    std::cout << "Unable to find <accessor> as a child of <technique_common> "
+          << "for texture coordinates with id[" << _id << "]\n";
+    return;
+  }
+
+  // Read in the total number of texture coordinate values
+  if (floatArrayXml->Attribute("count"))
+    totCount = boost::lexical_cast<int>(floatArrayXml->Attribute("count"));
+  else
+  {
+    std::cout << "<float_array> has no count attribute in texture coordinate "
+          << "element with id[" << _id << "]\n";
+    return;
+  }
+
+  // Read in the stride for the texture coordinate values. The stride
+  // indicates the number of values in the float array the comprise
+  // a complete texture coordinate.
+  if (xml->Attribute("stride"))
+    stride = boost::lexical_cast<int>(xml->Attribute("stride"));
+  else
+  {
+    std::cout << "<accessor> has no stride attribute in texture coordinate element "
+          << "with id[" << _id << "]\n";
+    return;
+  }
+
+  // Read in the count of texture coordinates.
+  if (xml->Attribute("count"))
+    texCount = boost::lexical_cast<int>(xml->Attribute("count"));
+  else
+  {
+    std::cout << "<accessor> has no count attribute in texture coordinate element "
+          << "with id[" << _id << "]\n";
+    return;
+  }
+
+  // \TODO This is a good a GZ_ASSERT
+  // The total number of texture values should equal the stride multiplied
+  // by the number of texture coordinates.
+  if (texCount * stride != totCount)
+  {
+    std::cout << "Error reading texture coordinates. Coordinate counts in element "
+             "with id[" << _id << "] do not add up correctly\n";
+    return;
+  }
+
+  // Read the raw texture values, and split them on spaces.
+  std::string valueStr = floatArrayXml->GetText();
+  std::vector<std::string> values;
+  boost::split(values, valueStr, boost::is_any_of(" "));
+
+  // Read in all the texture coordinates.
+  for (int i = 0; i < totCount; i += stride)
+  {
+    // We only handle 2D texture coordinates right now.
+    _values.push_back(math::Vector2d(boost::lexical_cast<double>(values[i]),
+          1.0 - boost::lexical_cast<double>(values[i+1])));
+  }
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadTriangles(TiXmlElement *_trianglesXml,
+                                  const math::Matriz4x4 &_transform,
+                                  Malla *_mesh)
+{
+  SubMalla *subMalla = new SubMalla;
+  subMalla->setName(this->currentNodeName);
+  bool combinedVertNorms = false;
+
+  subMalla->setPrimitiveType(SubMalla::TRIANGLES);
+
+  if (_trianglesXml->Attribute("material"))
+  {
+    std::map<std::string, std::string>::iterator iter;
+    std::string matStr = _trianglesXml->Attribute("material");
+
+    iter = this->materialMap.find(matStr);
+    if (iter != this->materialMap.end())
+      matStr = iter->second;
+
+    int matIndex = _mesh->addMaterial(this->loadMaterial(matStr));
+    if (matIndex < 0)
+      std::cout  << "Unable to add material[" << matStr << "]\n";
+    else
+      subMalla->setMaterialIndex(matIndex);
+  }
+
+  TiXmlElement *trianglesInputXml = _trianglesXml->FirstChildElement("input");
+
+  std::vector<math::Vector3> verts;
+  std::vector<math::Vector3> norms;
+  std::vector<math::Vector2d> texcoords;
+
+  // A list of all the input values.
+  std::list<std::pair<std::string, int> > inputs;
+  while (trianglesInputXml)
+  {
+    std::string semantic = trianglesInputXml->Attribute("semantic");
+    std::string source = trianglesInputXml->Attribute("source");
+    std::string offset = trianglesInputXml->Attribute("offset");
+    if (semantic == "VERTEX")
+    {
+      unsigned int count = norms.size();
+      this->loadVertices(source, _transform, verts, norms);
+      if (norms.size() > count)
+        combinedVertNorms = true;
+    }
+    else if (semantic == "NORMAL")
+    {
+      this->loadNormals(source, _transform, norms);
+      combinedVertNorms = false;
+    }
+    else if (semantic == "TEXCOORD")
+      this->loadTexCoords(source, texcoords);
+
+    inputs.push_back(std::make_pair(semantic, math::parseInt(offset)));
+
+    trianglesInputXml = trianglesInputXml->NextSiblingElement("input");
+  }
+
+  TiXmlElement *pXml = _trianglesXml->FirstChildElement("p");
+  if (!pXml || !pXml->GetText())
+  {
+    std::cout << "Collada file[" << this->filename
+          << "] is invalid. Loading what we can...\n";
+    return;
+  }
+  std::string pStr = pXml->GetText();
+
+  std::vector<math::Vector3> vertNorms(verts.size());
+  std::vector<int> vertNormsCounts(verts.size());
+  std::fill(vertNormsCounts.begin(), vertNormsCounts.end(), 0);
+
+  int *values = new int[inputs.size()];
+  std::list<std::pair<std::string, int> >::iterator end = inputs.end();
+  std::list<std::pair<std::string, int> >::iterator iter;
+  math::Vector2d vec;
+
+  std::vector<std::string> strs;
+  boost::split(strs, pStr, boost::is_any_of("   "));
+
+  for (unsigned int j = 0; j < strs.size(); j += inputs.size())
+  {
+    for (unsigned int i = 0; i < inputs.size(); i++)
+      values[i] = math::parseInt(strs[j+i]);
+
+    bool already = false;
+    for (iter = inputs.begin(); iter != end; ++iter)
+    {
+      if ((*iter).first == "VERTEX")
+      {
+        subMalla->addVertex(verts[values[(*iter).second]]);
+        subMalla->addIndex(subMalla->getVertexCount()-1);
+        if (combinedVertNorms)
+          subMalla->addNormal(norms[values[(*iter).second]]);
+//        if (_mesh->HasSkeleton())
+//        {
+//          Skeleton *skel = _mesh->GetSkeleton();
+//          for (unsigned int i = 0;
+//                  i < skel->GetNumVertNodeWeights(values[(*iter).second]); i++)
+//          {
+//            std::pair<std::string, double> node_weight =
+//              skel->GetVertNodeWeight(values[(*iter).second], i);
+//            SkeletonNode *node =
+//                _mesh->GetSkeleton()->GetNodeByName(node_weight.first);
+//            SubMalla->AddNodeAssignment(SubMalla->GetVertexCount()-1,
+//                            node->GetHandle(), node_weight.second);
+//          }
+//        }
+      }
+      else if ((*iter).first == "NORMAL")
+      {
+        subMalla->addNormal(norms[values[(*iter).second]]);
+      }
+      else if ((*iter).first == "TEXCOORD" && !already)
+      {
+        already = true;
+        subMalla->addTexCoord(texcoords[values[(*iter).second]].getX(),
+                              texcoords[values[(*iter).second]].getY());
+      }
+      // else
+      // gzerr << "Unhandled semantic[" << (*iter).first << "]\n";
+    }
+  }
+  delete [] values;
+
+  _mesh->addSubMalla(subMalla);
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadController(TiXmlElement *_contrXml,
+      TiXmlElement *_skelXml, const math::Matriz4x4 _transform, Malla *_mesh)
+{
+}
+//{
+//  Skeleton *skeleton = new Skeleton(this->LoadSkeletonNodes(_skelXml, NULL));
+//  _mesh->SetSkeleton(skeleton);
+
+//  TiXmlElement *rootXml = _contrXml->GetDocument()->RootElement();
+
+//  if (rootXml->FirstChildElement("library_animations"))
+//    this->LoadAnimations(rootXml->FirstChildElement("library_animations"),
+//        skeleton);
+
+//  TiXmlElement *skinXml = _contrXml->FirstChildElement("skin");
+//  std::string geomURL = skinXml->Attribute("source");
+
+//  math::Matrix4 bindTrans;
+//  std::string matrixStr =
+//        skinXml->FirstChildElement("bind_shape_matrix")->GetText();
+//  std::istringstream iss(matrixStr);
+//  std::vector<double> values(16);
+//  for (unsigned int i = 0; i < 16; i++)
+//    iss >> values[i];
+//  bindTrans.Set(values[0], values[1], values[2], values[3],
+//                values[4], values[5], values[6], values[7],
+//                values[8], values[9], values[10], values[11],
+//                values[12], values[13], values[14], values[15]);
+
+//  skeleton->SetBindShapeTransform(bindTrans);
+
+//  TiXmlElement *jointsXml = skinXml->FirstChildElement("joints");
+//  std::string jointsURL, invBindMatURL;
+//  TiXmlElement *inputXml = jointsXml->FirstChildElement("input");
+//  while (inputXml)
+//  {
+//    std::string semantic = inputXml->Attribute("semantic");
+//    std::string source = inputXml->Attribute("source");
+//    if (semantic == "JOINT")
+//      jointsURL = source;
+//    else
+//    {
+//      if (semantic == "INV_BIND_MATRIX")
+//        invBindMatURL = source;
+//    }
+//    inputXml = inputXml->NextSiblingElement("input");
+//  }
+
+//  jointsXml = this->GetElementId("source", jointsURL);
+
+//  if (!jointsXml)
+//  {
+//    gzerr << "Could not find node[" << jointsURL << "]\n";
+//    gzthrow("Faild to parse skinning information in Collada file.");
+//  }
+
+//  std::string jointsStr = jointsXml->FirstChildElement("Name_array")->GetText();
+
+//  std::vector<std::string> joints;
+//  boost::split(joints, jointsStr, boost::is_any_of("   "));
+
+//  TiXmlElement *invBMXml = this->GetElementId("source", invBindMatURL);
+
+//  if (!invBMXml)
+//  {
+//    gzerr << "Could not find node[" << invBindMatURL << "]\n";
+//    gzthrow("Faild to parse skinning information in Collada file.");
+//  }
+
+//  std::string posesStr = invBMXml->FirstChildElement("float_array")->GetText();
+
+//  std::vector<std::string> strs;
+//  boost::split(strs, posesStr, boost::is_any_of("   "));
+
+//  for (unsigned int i = 0; i < joints.size(); i++)
+//  {
+//    unsigned int id = i * 16;
+//    math::Matrix4 mat;
+//    mat.Set(math::parseFloat(strs[id +  0]), math::parseFloat(strs[id +  1]),
+//            math::parseFloat(strs[id +  2]), math::parseFloat(strs[id +  3]),
+//            math::parseFloat(strs[id +  4]), math::parseFloat(strs[id +  5]),
+//            math::parseFloat(strs[id +  6]), math::parseFloat(strs[id +  7]),
+//            math::parseFloat(strs[id +  8]), math::parseFloat(strs[id +  9]),
+//            math::parseFloat(strs[id + 10]), math::parseFloat(strs[id + 11]),
+//            math::parseFloat(strs[id + 12]), math::parseFloat(strs[id + 13]),
+//            math::parseFloat(strs[id + 14]), math::parseFloat(strs[id + 15]));
+
+//    skeleton->GetNodeByName(joints[i])->SetInverseBindTransform(mat);
+//  }
+
+//  TiXmlElement *vertWeightsXml = skinXml->FirstChildElement("vertex_weights");
+
+//  inputXml = vertWeightsXml->FirstChildElement("input");
+//  unsigned int jOffset = 0;
+//  unsigned int wOffset = 0;
+//  std::string weightsURL;
+//  while (inputXml)
+//  {
+//    std::string semantic = inputXml->Attribute("semantic");
+//    std::string source = inputXml->Attribute("source");
+//    int offset;
+//    inputXml->Attribute("offset", &offset);
+
+//    if (semantic == "JOINT")
+//      jOffset = offset;
+//    else
+//      if (semantic == "WEIGHT")
+//      {
+//        weightsURL = source;
+//        wOffset = offset;
+//      }
+//    inputXml = inputXml->NextSiblingElement("input");
+//  }
+
+//  TiXmlElement *weightsXml = this->GetElementId("source", weightsURL);
+
+//  std::string wString = weightsXml->FirstChildElement("float_array")->GetText();
+//  std::vector<std::string> wStrs;
+//  boost::split(wStrs, wString, boost::is_any_of("   "));
+
+//  std::vector<float> weights;
+//  for (unsigned int i = 0; i < wStrs.size(); i++)
+//    weights.push_back(math::parseFloat(wStrs[i]));
+
+//  std::string cString = vertWeightsXml->FirstChildElement("vcount")->GetText();
+//  std::string vString = vertWeightsXml->FirstChildElement("v")->GetText();
+//  std::vector<std::string> vCountStrs;
+//  std::vector<std::string> vStrs;
+
+//  boost::split(vCountStrs, cString, boost::is_any_of("   "));
+//  boost::split(vStrs, vString, boost::is_any_of("   "));
+
+//  std::vector<unsigned int> vCount;
+//  std::vector<unsigned int> v;
+
+//  for (unsigned int i = 0; i < vCountStrs.size(); i++)
+//    vCount.push_back(math::parseInt(vCountStrs[i]));
+
+//  for (unsigned int i = 0; i < vStrs.size(); i++)
+//    v.push_back(math::parseInt(vStrs[i]));
+
+//  skeleton->SetNumVertAttached(vCount.size());
+
+//  unsigned int vIndex = 0;
+//  for (unsigned int i = 0; i < vCount.size(); i++)
+//  {
+//    for (unsigned int j = 0; j < vCount[i]; j++)
+//    {
+//      skeleton->AddVertNodeWeight(i, joints[v[vIndex + jOffset]],
+//                                    weights[v[vIndex + wOffset]]);
+//      vIndex += (jOffset + wOffset + 1);
+//    }
+//  }
+
+//  TiXmlElement *geomXml = this->GetElementId("geometry", geomURL);
+//  this->LoadGeometry(geomXml, _transform, _mesh);
+//}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadColorOrTexture(TiXmlElement *_elem,
+    const std::string &_type, Material *_mat)
+{
+  if (!_elem || !_elem->FirstChildElement(_type))
+    return;
+
+  TiXmlElement *typeElem = _elem->FirstChildElement(_type);
+
+  if (typeElem->FirstChildElement("color"))
+  {
+    std::string colorStr = typeElem->FirstChildElement("color")->GetText();
+    Color color = boost::lexical_cast<Color>(colorStr);
+    if (_type == "diffuse")
+      _mat->setDiffuse(color);
+    else if (_type == "ambient")
+      _mat->setAmbient(color);
+    else if (_type == "emission")
+      _mat->setEmissive(color);
+  }
+  else if (typeElem->FirstChildElement("texture"))
+  {
+    TiXmlElement *imageXml = NULL;
+    std::string textureName =
+      typeElem->FirstChildElement("texture")->Attribute("texture");
+    TiXmlElement *textureXml = this->getElementId("newparam", textureName);
+    if (textureXml)
+    {
+      if (std::string(textureXml->Value()) == "image")
+      {
+        imageXml = textureXml;
+      }
+      else
+      {
+        TiXmlElement *sampler = textureXml->FirstChildElement("sampler2D");
+        if (sampler)
+        {
+          std::string sourceName =
+            sampler->FirstChildElement("source")->GetText();
+          TiXmlElement *sourceXml = this->getElementId("newparam", sourceName);
+          if (sourceXml)
+          {
+            TiXmlElement *surfaceXml = sourceXml->FirstChildElement("surface");
+            if (surfaceXml && surfaceXml->FirstChildElement("init_from"))
+            {
+              imageXml = this->getElementId("image",
+                  surfaceXml->FirstChildElement("init_from")->GetText());
+            }
+          }
+        }
+      }
+    }
+    else
+    {
+      imageXml = this->getElementId("image", textureName);
+    }
+
+    if (imageXml && imageXml->FirstChildElement("init_from"))
+    {
+      std::string imgFile =
+        imageXml->FirstChildElement("init_from")->GetText();
+
+      _mat->setTextureImage(imgFile, this->path);
+    }
+  }
+}
+
+float ColladaParser::loadFloat(TiXmlElement *_elem)
+{
+  float value = 0;
+
+  if (_elem->FirstChildElement("float"))
+  {
+    value = math::parseFloat(_elem->FirstChildElement("float")->GetText());
+  }
+
+  return value;
+}
+
+/////////////////////////////////////////////////
+void ColladaParser::loadTransparent(TiXmlElement *_elem, Material *_mat)
+{
+  const char *opaqueCStr = _elem->Attribute("opaque");
+  if (!opaqueCStr)
+  {
+    // gzerr << "No Opaque set\n";
+    return;
+  }
+
+  // TODO: Handle transparent textures
+  if (_elem->FirstChildElement("color"))
+  {
+    const char *colorCStr = _elem->FirstChildElement("color")->GetText();
+    if (!colorCStr)
+    {
+      std::cout  << "No color string\n";
+      return;
+    }
+
+    std::string opaqueStr = opaqueCStr;
+    std::string colorStr = colorCStr;
+    Color color = boost::lexical_cast<Color>(colorStr);
+
+    double srcFactor = 0;
+    double dstFactor = 0;
+
+    if (opaqueStr == "RGB_ZERO")
+    {
+      srcFactor = color.r * _mat->getTransparency();
+      dstFactor = 1.0 - color.r * _mat->getTransparency();
+    }
+    else if (opaqueStr == "A_ONE")
+    {
+      srcFactor = 1.0 - color.a * _mat->getTransparency();
+      dstFactor = color.a * _mat->getTransparency();
+    }
+
+    _mat->setBlendFactors(srcFactor, dstFactor);
+  }
+}
+
+
+/////////////////////////////////////////////////
+Material *ColladaParser::loadMaterial(const std::string &_name)
+{
+  TiXmlElement *matXml = this->getElementId("material", _name);
+  if (!matXml || !matXml->FirstChildElement("instance_effect"))
+    return NULL;
+
+  Material *mat = new Material();
+  std::string effectName =
+    matXml->FirstChildElement("instance_effect")->Attribute("url");
+  TiXmlElement *effectXml = this->getElementId("effect", effectName);
+
+  TiXmlElement *commonXml = effectXml->FirstChildElement("profile_COMMON");
+  if (commonXml)
+  {
+    TiXmlElement *techniqueXml = commonXml->FirstChildElement("technique");
+    TiXmlElement *lambertXml = techniqueXml->FirstChildElement("lambert");
+
+    TiXmlElement *phongXml = techniqueXml->FirstChildElement("phong");
+    TiXmlElement *blinnXml = techniqueXml->FirstChildElement("blinn");
+    if (lambertXml)
+    {
+      this->loadColorOrTexture(lambertXml, "ambient", mat);
+      this->loadColorOrTexture(lambertXml, "emission", mat);
+      this->loadColorOrTexture(lambertXml, "diffuse", mat);
+      if (lambertXml->FirstChildElement("transparency"))
+      {
+        mat->setTransparency(
+            this->loadFloat(lambertXml->FirstChildElement("transparency")));
+      }
+
+      if (lambertXml->FirstChildElement("transparent"))
+      {
+        TiXmlElement *transXml = lambertXml->FirstChildElement("transparent");
+        this->loadTransparent(transXml, mat);
+      }
+    }
+    else if (phongXml)
+    {
+      this->loadColorOrTexture(phongXml, "ambient", mat);
+      this->loadColorOrTexture(phongXml, "emission", mat);
+      this->loadColorOrTexture(phongXml, "specular", mat);
+      this->loadColorOrTexture(phongXml, "diffuse", mat);
+      if (phongXml->FirstChildElement("shininess"))
+        mat->setShininess(
+            this->loadFloat(phongXml->FirstChildElement("shininess")));
+
+      if (phongXml->FirstChildElement("transparency"))
+        mat->setTransparency(
+            this->loadFloat(phongXml->FirstChildElement("transparency")));
+      if (phongXml->FirstChildElement("transparent"))
+      {
+        TiXmlElement *transXml = phongXml->FirstChildElement("transparent");
+        this->loadTransparent(transXml, mat);
+      }
+    }
+    else if (blinnXml)
+    {
+      this->loadColorOrTexture(blinnXml, "ambient", mat);
+      this->loadColorOrTexture(blinnXml, "emission", mat);
+      this->loadColorOrTexture(blinnXml, "specular", mat);
+      this->loadColorOrTexture(blinnXml, "diffuse", mat);
+      if (blinnXml->FirstChildElement("shininess"))
+        mat->setShininess(
+            this->loadFloat(blinnXml->FirstChildElement("shininess")));
+
+      if (blinnXml->FirstChildElement("transparency"))
+        mat->setTransparency(
+            this->loadFloat(blinnXml->FirstChildElement("transparency")));
+      if (blinnXml->FirstChildElement("transparent"))
+      {
+        TiXmlElement *transXml = blinnXml->FirstChildElement("transparent");
+        this->loadTransparent(transXml, mat);
+      }
+    }
+  }
+
+  TiXmlElement *glslXml = effectXml->FirstChildElement("profile_GLSL");
+  if (glslXml)
+    std::cout  << "profile_GLSL unsupported\n";
+
+  TiXmlElement *cgXml = effectXml->FirstChildElement("profile_CG");
+  if (cgXml)
+    std::cout  << "profile_CG unsupported\n";
+  return mat;
+}
+

Added: trunk/src/libs/geometry/collada/colladaparser.h
===================================================================
--- trunk/src/libs/geometry/collada/colladaparser.h	                        (rev 0)
+++ trunk/src/libs/geometry/collada/colladaparser.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,107 @@
+#ifndef COLLADAPARSER_H
+#define COLLADAPARSER_H
+
+#include <iostream>
+#include <vector>
+#include <map>
+
+#include <tinyxml.h>
+
+#include <boost/lexical_cast.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string.hpp>
+
+#include "malla.h"
+#include "submalla.h"
+#include "color.h"
+
+#include "math/utils.h"
+#include "math/matriz4x4.h"
+#include "math/matriz3x3.h"
+#include "math/vector3.h"
+
+#include <GL/glut.h>
+#include <GL/gl.h>
+
+class ColladaParser
+{
+public:
+    ColladaParser(std::string filename);
+
+    void loadScene(Malla *_mesh);
+
+    void draw();
+
+    TiXmlElement *getElementId(const std::string &_name,
+                               const std::string &_id);
+
+    TiXmlElement *getElementId(TiXmlElement *_parent,
+                                const std::string &_name,
+                                const std::string &_id);
+
+    float loadFloat(TiXmlElement *_elem);
+    void loadTransparent(TiXmlElement *_elem, Material *_mat);
+
+
+    void loadNode(TiXmlElement *_elem,
+                  Malla *_mesh,
+                  const math::Matriz4x4 &_transform);
+
+    void loadColorOrTexture(TiXmlElement *_elem,
+        const std::string &_type, Material *_mat);
+
+    math::Matriz4x4 loadNodeTransform(TiXmlElement *_elem);
+
+    void loadGeometry(TiXmlElement *_xml,
+                      const math::Matriz4x4 &_transform, Malla *_mesh);
+
+    void loadVertices(const std::string &_id,
+                    const math::Matriz4x4 &_transform,
+                    std::vector<math::Vector3> &_verts,
+                    std::vector<math::Vector3> &_norms);
+
+    void loadNormals(const std::string &_id,
+                                    const math::Matriz4x4 &_transform,
+                                    std::vector<math::Vector3> &_values);
+
+    void loadPositions(const std::string &_id,
+                        const math::Matriz4x4 &_transform,
+                        std::vector<math::Vector3> &_values);
+
+    void loadController(TiXmlElement *_contrXml,
+          TiXmlElement *_skelXml, const math::Matriz4x4 _transform, Malla *_mesh);
+
+    void loadPolylist(TiXmlElement *_polylistXml,
+                    const math::Matriz4x4 &_transform,
+                    Malla *_mesh);
+
+    void loadLines(TiXmlElement *_xml,
+                    const math::Matriz4x4 &_transform,
+                    Malla *_mesh);
+
+    void loadTriangles(TiXmlElement *_trianglesXml,
+                                      const math::Matriz4x4 &_transform,
+                                      Malla *_mesh);
+    Material *loadMaterial(const std::string &_name);
+    void loadTexCoords(const std::string &_id,
+                                      std::vector<math::Vector2d> &_values);
+    // collada filename
+    private: std::string filename;
+    private: std::string path;
+
+    // root xml element
+    private: TiXmlElement *colladaXml;
+
+    // scale factor
+    private: double meter;
+
+    // Name of the current node.
+    private: std::string currentNodeName;
+
+    // material dictionary indexed by name
+    private: std::map<std::string, std::string> materialMap;
+
+public: Malla *mesh;
+};
+
+#endif // COLLADAPARSER_H

Added: trunk/src/libs/geometry/collada/color.cpp
===================================================================
--- trunk/src/libs/geometry/collada/color.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/collada/color.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,15 @@
+#include "color.h"
+
+Color::Color()
+{
+}
+
+
+void Color::set(float r, float g, float b, float a)
+{
+    this->r = r;
+    this->g = g;
+    this->b = b;
+    this->a = a;
+}
+

Added: trunk/src/libs/geometry/collada/color.h
===================================================================
--- trunk/src/libs/geometry/collada/color.h	                        (rev 0)
+++ trunk/src/libs/geometry/collada/color.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,25 @@
+#ifndef COLOR_H
+#define COLOR_H
+
+#include <iostream>
+
+class Color
+{
+public:
+    Color();
+    void set(float r, float g, float b, float a);
+
+
+    public: float r, g, b, a;
+
+    public: friend std::istream &operator>> (std::istream &_in, Color &_pt)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+      _in >> _pt.r >> _pt.g >> _pt.b >> _pt.a;
+      return _in;
+    }
+
+};
+
+#endif // COLOR_H

Added: trunk/src/libs/geometry/collada/malla.cpp
===================================================================
--- trunk/src/libs/geometry/collada/malla.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/collada/malla.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,108 @@
+#include "malla.h"
+
+Malla::Malla()
+{
+}
+
+//////////////////////////////////////////////////
+void Malla::setPath(const std::string &_path)
+{
+  this->path = _path;
+}
+
+//////////////////////////////////////////////////
+void Malla::addSubMalla(SubMalla *_sub)
+{
+  this->submeshes.push_back(_sub);
+}
+
+//////////////////////////////////////////////////
+int Malla::addMaterial(Material *_mat)
+{
+  int result = -1;
+
+  if (_mat)
+  {
+    this->materials.push_back(_mat);
+    result = this->materials.size()-1;
+  }
+
+  return result;
+}
+
+//////////////////////////////////////////////////
+Material *Malla::getMaterial(int index)
+{
+  if (index >= 0 && index < static_cast<int>(this->materials.size()))
+    return this->materials[index];
+
+  return NULL;
+}
+
+//////////////////////////////////////////////////
+unsigned int Malla::getSubMeshCount() const
+{
+  return this->submeshes.size();
+}
+
+//////////////////////////////////////////////////
+SubMalla *Malla::getSubMesh(unsigned int i) const
+{
+  if (i < this->submeshes.size())
+    return this->submeshes[i];
+  else
+    std::cout << "Invalid index: " << i << " >= " << this->submeshes.size() << "\n";
+}
+
+//////////////////////////////////////////////////
+math::Vector3 Malla::getMax() const
+{
+  math::Vector3 max;
+  std::vector<SubMalla*>::const_iterator iter;
+
+  max.setX( -std::numeric_limits<float>::max());
+  max.setY( -std::numeric_limits<float>::max());
+  max.setZ( -std::numeric_limits<float>::max());
+
+  for (iter = this->submeshes.begin(); iter != this->submeshes.end(); ++iter)
+  {
+    if ((*iter)->getVertexCount() <= 2)
+      continue;
+
+    math::Vector3 smax = (*iter)->getMax();
+
+    max.setX(std::max(max.getX(), smax.getX()));
+    max.setY(std::max(max.getY(), smax.getY()));
+    max.setZ(std::max(max.getZ(), smax.getZ()));
+  }
+
+  return max;
+}
+
+//////////////////////////////////////////////////
+math::Vector3 Malla::getMin() const
+{
+  math::Vector3 min;
+  std::vector<SubMalla *>::const_iterator iter;
+
+  min.setX( std::numeric_limits<float>::max());
+  min.setY( std::numeric_limits<float>::max());
+  min.setZ( std::numeric_limits<float>::max());
+
+  for (iter = this->submeshes.begin(); iter != this->submeshes.end(); ++iter)
+  {
+    if ((*iter)->getVertexCount() <= 2)
+      continue;
+
+    math::Vector3 smin = (*iter)->getMin();
+    min.setX(std::min(min.getX(), smin.getX()));
+    min.setY(std::min(min.getY(), smin.getY()));
+    min.setZ(std::min(min.getZ(), smin.getZ()));
+  }
+
+  return min;
+}
+
+
+
+

Added: trunk/src/libs/geometry/collada/malla.h
===================================================================
--- trunk/src/libs/geometry/collada/malla.h	                        (rev 0)
+++ trunk/src/libs/geometry/collada/malla.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,35 @@
+#ifndef MALLA_H
+#define MALLA_H
+
+#include <iostream>
+#include <vector>
+
+#include "submalla.h"
+#include "material.h"
+
+class Malla
+{
+public:
+    Malla();
+
+    void setPath(const std::string &_path);
+    void addSubMalla(SubMalla *_sub);
+    int addMaterial(Material *_mat);
+    unsigned int getSubMeshCount() const;
+    SubMalla *getSubMesh(unsigned int i) const;
+    Material *getMaterial(int index);
+
+    math::Vector3 getMax() const;
+    math::Vector3 getMin() const;
+
+
+    /// \brief The name of the mesh
+    private: std::string name;
+
+    private: std::string path;
+
+    private: std::vector<SubMalla *> submeshes;
+    private: std::vector<Material *> materials;
+};
+
+#endif // MALLA_H

Added: trunk/src/libs/geometry/collada/material.cpp
===================================================================
--- trunk/src/libs/geometry/collada/material.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/collada/material.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,139 @@
+#include "material.h"
+
+unsigned int Material::counter = 0;
+
+Material::Material()
+{
+  this->name = "gazebo_material_" + boost::lexical_cast<std::string>(counter++);
+  this->specular.set(0, 0, 0, 1);
+  this->lighting = false;
+}
+
+//////////////////////////////////////////////////
+void Material::setDiffuse(const Color &_clr)
+{
+  this->diffuse = _clr;
+  this->lighting = true;
+}
+//////////////////////////////////////////////////
+Color Material::getDiffuse() const
+{
+  return this->diffuse;
+}
+
+//////////////////////////////////////////////////
+void Material::setAmbient(const Color &_clr)
+{
+  this->ambient = _clr;
+}
+
+//////////////////////////////////////////////////
+Color Material::getAmbient() const
+{
+  return this->ambient;
+}
+
+//////////////////////////////////////////////////
+void Material::setEmissive(const Color &_clr)
+{
+  this->emissive = _clr;
+}
+
+//////////////////////////////////////////////////
+Color Material::getEmissive() const
+{
+  return this->emissive;
+}
+
+//////////////////////////////////////////////////
+double Material::getShininess() const
+{
+  return this->shininess;
+}
+
+
+//////////////////////////////////////////////////
+bool Material::getLighting() const
+{
+  return this->lighting;
+}
+
+//////////////////////////////////////////////////
+void Material::setTextureImage(const std::string &_tex)
+{
+  this->texImage = _tex;
+}
+
+//////////////////////////////////////////////////
+void Material::setTextureImage(const std::string &_tex,
+                               const std::string &_resourcePath)
+{
+  this->texImage = _resourcePath + "/" + _tex;
+
+  // If the texture image doesn't exist then try the next most likely path.
+  if (!boost::filesystem::exists(this->texImage))
+  {
+    this->texImage = _resourcePath + "./" + _tex;
+    if (!boost::filesystem::exists(this->texImage))
+    {
+      std::cout << "Unable to find texture[" << _tex << "] in path["
+            << _resourcePath << "]\n";
+    }else{
+        std::cout << "find texture[" << _tex << "] in path["
+              << _resourcePath << "]\n";
+        image = cv::imread(this->texImage);
+    }
+  }
+}
+
+cv::Mat Material::getImage()
+{
+    return image;
+}
+
+//////////////////////////////////////////////////
+void Material::setTransparency(double _t)
+{
+  this->transparency = std::min(_t, 1.0);
+  this->transparency = std::max(this->transparency, 0.0);
+  this->lighting = true;
+}
+
+//////////////////////////////////////////////////
+Color Material::getSpecular() const
+{
+  return this->specular;
+}
+
+//////////////////////////////////////////////////
+double Material::getTransparency() const
+{
+  return this->transparency;
+}
+
+//////////////////////////////////////////////////
+void Material::setBlendFactors(double _srcFactor, double _dstFactor)
+{
+  this->srcBlendFactor = _srcFactor;
+  this->dstBlendFactor = _dstFactor;
+}
+
+//////////////////////////////////////////////////
+void Material::setShininess(double _s)
+{
+  this->shininess = _s;
+  this->lighting = true;
+}
+
+//////////////////////////////////////////////////
+std::string Material::getTextureImage() const
+{
+  return this->texImage;
+}
+
+//////////////////////////////////////////////////
+std::string Material::getName() const
+{
+  return this->name;
+}
+

Added: trunk/src/libs/geometry/collada/material.h
===================================================================
--- trunk/src/libs/geometry/collada/material.h	                        (rev 0)
+++ trunk/src/libs/geometry/collada/material.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,95 @@
+#ifndef MATERIAL_H
+#define MATERIAL_H
+
+#include "color.h"
+#include <string>
+
+#include <boost/filesystem.hpp>
+#include <boost/lexical_cast.hpp>
+
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+class Material
+{
+public:
+    Material();
+
+    public: void setDiffuse(const Color &_clr);
+    Color getDiffuse() const;
+
+    void setAmbient(const Color &_clr);
+    Color getAmbient() const;
+
+    void setEmissive(const Color &_clr);
+    Color getEmissive() const;
+
+    bool getLighting() const;
+    double getShininess() const;
+
+    std::string getTextureImage() const;
+
+
+    void setTextureImage(const std::string &_tex);
+    void setTextureImage(const std::string &_tex,
+                                   const std::string &_resourcePath);
+    void setTransparency(double _t);
+    double getTransparency() const;
+    void setBlendFactors(double _srcFactor, double _dstFactor);
+    void setShininess(double _s);
+
+    Color getSpecular() const;
+
+
+    std::string getName() const;
+
+
+    cv::Mat getImage();
+
+
+///////////////////////////////////////////////////////////////////////////
+    /// \brief the name of the material
+     public: std::string name;
+
+     /// \brief the texture image file name
+     public: std::string texImage;
+
+     /// \brief the ambient light color
+     public: Color ambient;
+
+     /// \brief the diffuse ligth color
+     public: Color diffuse;
+
+     /// \brief the specular light color
+     public: Color specular;
+
+     /// \brief the emissive light color
+     public: Color emissive;
+
+     /// \brief transparency value in the range 0 to 1
+     public: double transparency;
+
+     /// \brief shininess value (0 to 1)
+     public: double shininess;
+
+    /// \brief the total number of instanciated Material instances
+    private: static unsigned int counter;
+
+    /// \brief flag to perform depth buffer write
+    private: bool depthWrite;
+
+    private: bool lighting;
+
+    /// \brief source blend factor
+    private: double srcBlendFactor;
+
+    /// \brief destination blend factor
+    private: double dstBlendFactor;
+
+    public: unsigned int idTextura ;
+
+    cv::Mat image;
+};
+
+#endif // MATERIAL_H

Added: trunk/src/libs/geometry/collada/submalla.cpp
===================================================================
--- trunk/src/libs/geometry/collada/submalla.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/collada/submalla.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,228 @@
+#include "submalla.h"
+
+SubMalla::SubMalla()
+{
+    materialIndex = -1;
+}
+
+SubMalla::~SubMalla()
+{
+}
+
+//////////////////////////////////////////////////
+void SubMalla::setName(const std::string &_n)
+{
+  this->name = _n;
+}
+
+//////////////////////////////////////////////////
+void SubMalla::setPrimitiveType(PrimitiveType _type)
+{
+  this->primitiveType = _type;
+}
+
+//////////////////////////////////////////////////
+int SubMalla::getPrimitiveType() const
+{
+  return this->primitiveType;
+}
+
+//////////////////////////////////////////////////
+void SubMalla::addVertex(const math::Vector3 &_v)
+{
+  this->vertices.push_back(_v);
+}
+
+//////////////////////////////////////////////////
+void SubMalla::addIndex(unsigned int _i)
+{
+  this->indices.push_back(_i);
+}
+
+//////////////////////////////////////////////////
+unsigned int SubMalla::getVertexCount() const
+{
+  return this->vertices.size();
+}
+
+//////////////////////////////////////////////////
+math::Vector3 SubMalla::getVertex(unsigned int _i) const
+{
+  if (_i >= this->vertices.size())
+    std::cout << "Index too large\n";
+
+  return this->vertices[_i];
+}
+
+//////////////////////////////////////////////////
+math::Vector3 SubMalla::getNormal(unsigned int _i) const
+{
+  if (_i >= this->normals.size())
+    std::cout <<"Index too large";
+
+  return this->normals[_i];
+}
+
+//////////////////////////////////////////////////
+unsigned int SubMalla::getNormalCount() const
+{
+  return this->normals.size();
+}
+
+//////////////////////////////////////////////////
+void SubMalla::setMaterialIndex(unsigned int _index)
+{
+  this->materialIndex = _index;
+}
+
+//////////////////////////////////////////////////
+unsigned int SubMalla::getMaterialIndex() const
+{
+  return this->materialIndex;
+}
+
+//////////////////////////////////////////////////
+void SubMalla::addNormal(const math::Vector3 &_n)
+{
+  this->normals.push_back(_n);
+}
+
+//////////////////////////////////////////////////
+void SubMalla::addTexCoord(double _u, double _v)
+{
+  this->texCoords.push_back(math::Vector2d(_u, _v));
+}
+
+//////////////////////////////////////////////////
+unsigned int SubMalla::getTexCoordCount() const
+{
+  return this->texCoords.size();
+}
+
+//////////////////////////////////////////////////
+unsigned int SubMalla::getIndexCount() const
+{
+  return this->indices.size();
+}
+
+
+//////////////////////////////////////////////////
+unsigned int SubMalla::getIndex(unsigned int _i) const
+{
+  if (_i > this->indices.size())
+    std::cout << "Index too large";
+
+  return this->indices[_i];
+}
+
+//////////////////////////////////////////////////
+math::Vector2d SubMalla::getTexCoord(unsigned int _i) const
+{
+  if (_i >= this->texCoords.size())
+    std::cout << "Index too large";
+
+  return this->texCoords[_i];
+}
+
+//////////////////////////////////////////////////
+void SubMalla::center()
+{
+ math::Vector3 _center(0, 0, 0);
+
+  math::Vector3 min, max, half;
+  min = this->getMin();
+  max = this->getMax();
+  half = (max - min) * 0.5;
+
+  this->translate(_center - (min + half));
+}
+
+//////////////////////////////////////////////////
+void SubMalla::translate(const math::Vector3 &_vec)
+{
+  for (std::vector<math::Vector3>::iterator iter = this->vertices.begin();
+       iter != this->vertices.end(); ++iter)
+  {
+    (*iter) += _vec;
+  }
+}
+
+//////////////////////////////////////////////////
+math::Vector3 SubMalla::getMax() const
+{
+  math::Vector3 max;
+  std::vector<math::Vector3>::const_iterator iter;
+
+  max.setX( -std::numeric_limits<float>::max());
+  max.setY( -std::numeric_limits<float>::max());
+  max.setZ( -std::numeric_limits<float>::max());
+
+  for (iter = this->vertices.begin(); iter != this->vertices.end(); ++iter)
+  {
+    math::Vector3 v = (*iter);
+    max.setX(std::max(max.getX(), v.getX()));
+    max.setY(std::max(max.getY(), v.getY()));
+    max.setZ(std::max(max.getZ(), v.getZ()));
+  }
+
+  return max;
+}
+
+//////////////////////////////////////////////////
+math::Vector3 SubMalla::getMin() const
+{
+  math::Vector3 min;
+  std::vector<math::Vector3>::const_iterator iter;
+
+  min.setX( std::numeric_limits<float>::max());
+  min.setY( std::numeric_limits<float>::max());
+  min.setZ( std::numeric_limits<float>::max());
+
+  for (iter = this->vertices.begin(); iter != this->vertices.end(); ++iter)
+  {
+    math::Vector3 v = (*iter);
+    min.setX(std::min(min.getX(), v.getX()));
+    min.setY(std::min(min.getY(), v.getY()));
+    min.setZ(std::min(min.getZ(), v.getZ()));
+
+  }
+
+  return min;
+}
+
+//////////////////////////////////////////////////
+// PAra ODE
+////
+void SubMalla::FillArrays(float **_vertArr, int **_indArr)
+{
+  if (this->vertices.size() == 0 || this->indices.size() == 0)
+    std::cout << "No vertices or indices\n";
+
+  std::vector< math::Vector3 >::const_iterator viter;
+  std::vector< unsigned int >::const_iterator iiter;
+  unsigned int i;
+
+  if (*_vertArr)
+    delete [] *_vertArr;
+
+  if (*_indArr)
+    delete [] *_indArr;
+
+  *_vertArr = new float[this->vertices.size() * 3];
+  *_indArr = new int[this->indices.size()];
+
+  for (viter = this->vertices.begin(), i = 0; viter != this->vertices.end();
+      ++viter){
+      (*_vertArr)[i++] = static_cast<float>((*viter).vector(0));
+      (*_vertArr)[i++] = static_cast<float>((*viter).vector(1));
+      (*_vertArr)[i++] = static_cast<float>((*viter).vector(2));
+  }
+
+  for (iiter = this->indices.begin(), i = 0;
+      iiter != this->indices.end(); ++iiter)
+    (*_indArr)[i++] = (*iiter);
+}
+
+
+
+

Added: trunk/src/libs/geometry/collada/submalla.h
===================================================================
--- trunk/src/libs/geometry/collada/submalla.h	                        (rev 0)
+++ trunk/src/libs/geometry/collada/submalla.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,82 @@
+#ifndef SUBMALLA_H
+#define SUBMALLA_H
+
+#include <vector>
+
+#include "math/vector3.h"
+#include "math/vector2d.h"
+
+class SubMalla
+{
+
+    /// \brief An enumeration of the geometric mesh primitives
+    public: enum PrimitiveType {POINTS, LINES, LINESTRIPS, TRIANGLES,
+              TRIFANS, TRISTRIPS};
+
+
+public:
+    SubMalla();
+
+    public: virtual ~SubMalla();
+
+    public: void setName(const std::string &_n);
+
+    public: void setPrimitiveType(PrimitiveType _type);
+    int getPrimitiveType() const;
+
+
+    void addVertex(const math::Vector3 &_v);
+    void addIndex(unsigned int _i);
+    unsigned int getVertexCount() const;
+
+    void addNormal(const math::Vector3 &_n);
+    unsigned int getNormalCount() const;
+
+
+    void addTexCoord(double _u, double _v);
+    unsigned int getTexCoordCount() const;
+
+    math::Vector3 getVertex(unsigned int _i) const;
+    math::Vector3 getNormal(unsigned int _i) const;
+    math::Vector2d getTexCoord(unsigned int _i) const;
+
+    unsigned int getIndex(unsigned int _i) const;
+    unsigned int getIndexCount() const;
+
+    public: void setMaterialIndex(unsigned int _index);
+    unsigned int getMaterialIndex() const;
+
+
+
+    void center();
+    math::Vector3 getMax() const;
+    math::Vector3 getMin() const;
+
+    void translate(const math::Vector3 &_vec);
+
+    void FillArrays(float **_vertArr, int **_indArr) ;
+
+    ///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+    /// \brief the vertex array
+    private: std::vector< math::Vector3 > vertices;
+
+    /// \brief the normal array
+    private: std::vector< math::Vector3 > normals;
+
+    /// \brief the texture coordinate array
+    private: std::vector< math::Vector2d > texCoords;
+
+    /// \brief the vertex index array
+    private: std::vector<unsigned int> indices;
+
+    /// mesh material list.
+    private: int materialIndex;
+
+    /// \brief The name of the sub-mesh
+    private: std::string name;
+
+    private: PrimitiveType primitiveType;
+};
+
+#endif // SUBMALLA_H

Added: trunk/src/libs/geometry/math/matriz3x3.cpp
===================================================================
--- trunk/src/libs/geometry/math/matriz3x3.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/matriz3x3.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,65 @@
+#include "matriz3x3.h"
+
+namespace math
+{
+    Matriz3x3::Matriz3x3()
+    {
+        matriz   << 1, 0, 0,
+                    0, 1, 0,
+                    0, 0, 1;
+    }
+
+    Eigen::Matrix3f Matriz3x3::getMatriz()
+    {
+        return this->matriz;
+    }
+
+    Matriz3x3 Matriz3x3::operator*(const Matriz3x3 &_m) const
+    {
+        Matriz3x3 m;
+        m.matriz = this->matriz*_m.matriz;
+        return m;
+    }
+
+//    //////////////////////////////////////////////////
+    void Matriz3x3::setFromAxis(float x, float y, float z, float angle)
+    {
+        if(x>0.1){
+
+            matriz << 1., 0.,                    0.,
+                        0., cos(angle), -sin(angle),
+                        0., sin(angle), cos(angle);
+
+        }
+
+        if(y>0.1){
+            matriz << cos(angle ),   0.,     sin(angle),
+                        0.,                     1.0,    0.,
+                        -sin(angle),   0.0,    cos(angle );
+        }
+
+        if(z>0.1){
+            matriz << cos(angle),   -sin(angle),    0.,
+                        sin(angle),    cos(angle),    0.,
+                        0.,                     0.0,                    1.0;
+        }
+
+//        std::cout << matriz << std::endl;
+
+//      float c = cos(_angle);
+//      float s = sin(_angle);
+//      float C = 1-c;
+
+//      matriz(0,0) = x*x*C + c;
+//      matriz(0,1) = x*y*C - z*s;
+//      matriz(0,2) = x*z*C + y*s;
+
+//      matriz(1,0) = y*x*C + z*s;
+//      matriz(1,1) = y*y*C + c;
+//      matriz(1,2) = y*z*C - x*s;
+
+//      matriz(2,0) = z*x*C - y*s;
+//      matriz(2,1) = z*y*C + x*s;
+//      matriz(2,2) = z*z*C + c;
+    }
+}

Added: trunk/src/libs/geometry/math/matriz3x3.h
===================================================================
--- trunk/src/libs/geometry/math/matriz3x3.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/matriz3x3.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,39 @@
+#ifndef MATRIZ3X3_H
+#define MATRIZ3X3_H
+
+#include <eigen3/Eigen/Dense>
+
+#include "vector3.h"
+
+namespace math
+{
+    class Matriz3x3
+    {
+    public: Matriz3x3();
+
+    public: Eigen::Matrix3f matriz;
+
+    public: Eigen::Matrix3f getMatriz();
+
+    public:
+        void setFromAxis(float x, float y, float z, float angle);
+        Matriz3x3 operator*(const Matriz3x3 &_m) const;
+
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                                             Matriz3x3 &_m)
+    {
+            for (int i = 0; i < 3; i++)
+            {
+              for (int j = 0; j < 3; j++)
+              {
+                _out << _m.getMatriz()(i,j) << " ";
+              }
+              _out << "\n";
+            }
+
+            return _out;
+    }
+    };
+}
+
+#endif // MATRIZ3X3_H

Added: trunk/src/libs/geometry/math/matriz4x4.cpp
===================================================================
--- trunk/src/libs/geometry/math/matriz4x4.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/matriz4x4.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,115 @@
+#include "matriz4x4.h"
+
+namespace math
+{
+
+    Matriz4x4::Matriz4x4()
+    {
+        matriz  <<  1, 0, 0, 0,
+                    0, 1, 0, 0,
+                    0, 0, 1, 0,
+                    0, 0, 0, 1;
+    }
+
+    Matriz4x4::Matriz4x4(int type)
+    {
+        if(type==IDENTITY){
+            matriz =Eigen::Matrix4f::Identity();
+        }else if(type==ZEROS){
+            matriz  << 0, 0, 0, 0,
+                    0, 0, 0, 0,
+                    0, 0, 0, 0,
+                    0, 0, 0, 0;
+        }else{
+            matriz =Eigen::Matrix4f::Identity();
+        }
+    }
+
+    //////////////////////////////////////////////////
+    void Matriz4x4::set(double _v00, double _v01, double _v02, double _v03,
+                      double _v10, double _v11, double _v12, double _v13,
+                      double _v20, double _v21, double _v22, double _v23,
+                      double _v30, double _v31, double _v32, double _v33)
+    {
+
+        matriz << _v00, _v01, _v02, _v03,
+                  _v10, _v11, _v12, _v13,
+                  _v20, _v21, _v22, _v23,
+                  _v30, _v31, _v32, _v33;
+    }
+
+    //////////////////////////////////////////////////
+    void Matriz4x4::setScale( Vector3 &_s)
+    {
+      matriz(0, 0) = _s.getX();
+      matriz(1, 1) = _s.getY();
+      matriz(2, 2) = _s.getZ();
+      matriz(3, 3) = 1.0;
+    }
+
+
+    //////////////////////////////////////////////////
+    void Matriz4x4::setTranslate( Vector3 &_t)
+    {
+        matriz(0, 3) = _t.getX();
+        matriz(1, 3) = _t.getY();
+        matriz(2, 3) = _t.getZ();
+    }
+
+    Matriz4x4 Matriz4x4::operator*(Matriz3x3 _mat) const
+    {
+
+        Matriz4x4 r(math::Matriz4x4::IDENTITY);
+
+        r.set( matriz(0, 0), matriz(0, 1), matriz(0, 2),matriz(0, 3),
+               matriz(1, 0), matriz(1, 1), matriz(1, 2),matriz(1, 3),
+               matriz(2, 0), matriz(2, 1), matriz(2, 2),matriz(2, 3),
+               matriz(3, 0), matriz(3, 1), matriz(3, 2),matriz(3, 3));
+
+        Eigen::Matrix3f m = r.matriz.block(0, 0, 3, 3);
+
+        Eigen::Matrix3f resultado = m*_mat.getMatriz();
+
+        r.set(resultado(0, 0), resultado(0, 1), resultado(0, 2),matriz(0, 3),
+              resultado(1, 0), resultado(1, 1), resultado(1, 2),matriz(1, 3),
+              resultado(2, 0), resultado(2, 1), resultado(2, 2),matriz(2, 3),
+              matriz(3, 0), matriz(3, 1), matriz(3, 2),matriz(3, 3));
+        return r;
+
+    }
+
+    Eigen::Matrix4f Matriz4x4::getMatrix()
+    {
+        return this->matriz;
+    }
+
+    void Matriz4x4::setMatrix(Eigen::Matrix4f m)
+    {
+        this->matriz = m;
+    }
+
+
+    Matriz4x4 Matriz4x4::operator*(Matriz4x4 &_mat) const
+    {
+        Eigen::Matrix4f m = _mat.getMatrix() * matriz;
+
+        Matriz4x4 result;
+        result.setMatrix(m);
+
+        return result;
+    }
+
+    Vector3 Matriz4x4::operator*(Vector3 &_vec) const
+    {
+        Vector3 result;
+        result.setX( matriz(0, 0)*_vec.getX() + matriz(0, 1)*_vec.getY() +
+                     matriz(0, 2)*_vec.getZ() + matriz(0, 3));
+
+        result.setY( matriz(1, 0)*_vec.getX() + matriz(1, 1)*_vec.getY() +
+                     matriz(1, 2)*_vec.getZ() + matriz(1, 3));
+
+        result.setZ( matriz(2, 0)*_vec.getX() + matriz(2, 1)*_vec.getY() +
+                     matriz(2, 2)*_vec.getZ() + matriz(2, 3));
+        return result;
+    }
+}

Added: trunk/src/libs/geometry/math/matriz4x4.h
===================================================================
--- trunk/src/libs/geometry/math/matriz4x4.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/matriz4x4.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,40 @@
+#ifndef MATRIZ4X4_H
+#define MATRIZ4X4_H
+
+#include <eigen3/Eigen/Dense>
+
+#include "vector3.h"
+#include "matriz3x3.h"
+
+namespace math
+{
+
+
+    class Matriz4x4
+    {
+    public: enum typeMatriz { IDENTITY, ZEROS };
+
+    public:
+        Matriz4x4();
+        Matriz4x4(int type);
+        Eigen::Matrix4f getMatrix();
+        void setMatrix(Eigen::Matrix4f m);
+
+        void setTranslate(Vector3 &_t);
+        void setScale(Vector3 &_s);
+
+        void set(double _v00, double _v01, double _v02, double _v03,
+                 double _v10, double _v11, double _v12, double _v13,
+                 double _v20, double _v21, double _v22, double _v23,
+                 double _v30, double _v31, double _v32, double _v33);
+
+        public: Matriz4x4 operator*(Matriz3x3 _mat) const;
+        public: Matriz4x4 operator*( Matriz4x4 &_mat) const;
+        public: Vector3 operator*(Vector3 &_vec) const;
+    private:
+        Eigen::Matrix4f matriz;
+    };
+
+}
+
+#endif // MATRIZ4X4_H

Added: trunk/src/libs/geometry/math/utils.cpp
===================================================================
--- trunk/src/libs/geometry/math/utils.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/utils.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,5 @@
+#include "utils.h"
+
+namespace math
+{
+}

Added: trunk/src/libs/geometry/math/utils.h
===================================================================
--- trunk/src/libs/geometry/math/utils.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/utils.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,113 @@
+#ifndef UTILS_H
+#define UTILS_H
+
+#include <iostream>
+#include <math.h>
+#include <limits>
+
+#include <boost/math/special_functions.hpp>
+
+namespace math
+{
+
+    template<typename T>
+    inline bool equal(const T &_a, const T &_b,
+                      const T &_epsilon = 1e-6)
+    {
+      return std::fabs(_a - _b) <= _epsilon;
+    }
+
+    inline double parseFloat(const std::string& _input)
+    {
+      const char *p = _input.c_str();
+      if (!*p || *p == '?')
+        return std::numeric_limits<double>::quiet_NaN();
+      int s = 1;
+      while (*p == ' ')
+        p++;
+
+      if (*p == '-')
+      {
+        s = -1;
+        p++;
+      }
+
+      double acc = 0;
+      while (*p >= '0' && *p <= '9')
+        acc = acc * 10 + *p++ - '0';
+
+      if (*p == '.')
+      {
+        double k = 0.1;
+        p++;
+        while (*p >= '0' && *p <= '9')
+        {
+          acc += (*p++ - '0') * k;
+          k *= 0.1;
+        }
+      }
+      if (*p == 'e')
+      {
+        int es = 1;
+        int f = 0;
+        p++;
+        if (*p == '-')
+        {
+          es = -1;
+          p++;
+        }
+        else if (*p == '+')
+        {
+          es = 1;
+          p++;
+        }
+        while (*p >= '0' && *p <= '9')
+          f = f * 10 + *p++ - '0';
+
+        acc *= pow(10, f*es);
+      }
+
+      if (*p)
+      {
+        std::cerr << "Invalid format[" << _input << "]\n";
+        return 0.0;
+      }
+      return s * acc;
+    }
+
+    inline int parseInt(const std::string& _input)
+        {
+          const char *p = _input.c_str();
+          if (!*p || *p == '?')
+            return  std::numeric_limits<int>::quiet_NaN();
+
+          int s = 1;
+          while (*p == ' ')
+            p++;
+
+          if (*p == '-')
+          {
+            s = -1;
+            p++;
+          }
+
+          double acc = 0;
+          while (*p >= '0' && *p <= '9')
+            acc = acc * 10 + *p++ - '0';
+
+          if (*p)
+          {
+            std::cerr << "Invalid int numeric format[" << _input << "]\n";
+            return 0.0;
+          }
+
+          return s * acc;
+        }
+
+    template<typename T>
+    inline T precision(const T &_a, const unsigned int &_precision)
+    {
+      return boost::math::round(_a * pow(10, _precision)) / pow(10, _precision);
+    }
+}
+#endif // UTILS_H

Added: trunk/src/libs/geometry/math/vector2d.cpp
===================================================================
--- trunk/src/libs/geometry/math/vector2d.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/vector2d.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,160 @@
+#include "vector2d.h"
+namespace math
+{
+    Vector2d::Vector2d()
+    {
+        vector << 0, 0;
+
+    }
+
+    Vector2d::Vector2d(float _x, float _y)
+    {
+        vector << _x, _y;
+
+    }
+
+    float Vector2d::getX()
+    {
+        return vector(0);
+    }
+
+    float Vector2d::getY()
+    {
+        return vector(1);
+    }
+
+    float Vector2d::getX() const
+    {
+        return vector(0);
+    }
+
+    float Vector2d::getY() const
+    {
+        return vector(1);
+    }
+
+    void Vector2d::setX(float _x)
+    {
+        vector(0) = _x;
+    }
+
+    void Vector2d::setY(float _y )
+    {
+        vector(1) = _y;
+
+    }
+
+    //////////////////////////////////////////////////
+    Vector2d Vector2d::operator-(const Vector2d &pt) const
+    {
+      return Vector2d(vector(0) - pt.vector(0), vector(1) - pt.vector(1));
+    }
+
+    const Vector2d &Vector2d::operator-=(const Vector2d &pt)
+    {
+      vector(0) -= pt.vector(0);
+      vector(1) -= pt.vector(1);
+
+      return *this;
+    }
+
+    //////////////////////////////////////////////////
+
+    const Vector2d Vector2d::operator/(const Vector2d &pt) const
+    {
+      return Vector2d(vector(0) / pt.vector(0), vector(1) / pt.vector(1));
+    }
+
+    const Vector2d &Vector2d::operator/=(const Vector2d &pt)
+    {
+      vector(0) /= pt.vector(0);
+      vector(1) /= pt.vector(1);
+
+      return *this;
+    }
+
+    const Vector2d Vector2d::operator/(double v) const
+    {
+      return Vector2d(vector(0) / v, vector(1) / v);
+    }
+
+    const Vector2d &Vector2d::operator/=(double v)
+    {
+      vector(0) /= v;
+      vector(1) /= v;
+
+      return *this;
+    }
+
+    double Vector2d::mag_squared() const
+    {
+        return (vector(0)*vector(0))*(vector(1)*vector(1));
+    }
+
+    double Vector2d::mag_squared()
+    {
+        return (vector(0)*vector(0))*(vector(1)*vector(1));
+    }
+
+
+    //////////////////////////////////////////////////
+    const Vector2d Vector2d::operator*(const Vector2d &pt) const
+    {
+      return Vector2d(vector(0) * pt.vector(0), vector(1) * pt.vector(1));
+    }
+
+    const Vector2d &Vector2d::operator*=(const Vector2d &pt)
+    {
+      vector(0) *= pt.vector(0);
+      vector(1) *= pt.vector(1);
+
+      return *this;
+    }
+
+    const Vector2d Vector2d::operator*(double v) const
+    {
+      return Vector2d(vector(0) * v, vector(1) * v);
+    }
+
+    const Vector2d &Vector2d::operator*=(double v)
+    {
+      vector(0) *= v;
+      vector(1) *= v;
+
+      return *this;
+    }
+
+    //////////////////////////////////////////////////
+    Vector2d &Vector2d::operator =(const Vector2d &pt)
+    {
+      vector(0) = pt.vector(0);
+      vector(1) = pt.vector(1);
+
+      return *this;
+    }
+
+    //////////////////////////////////////////////////
+    const Vector2d &Vector2d::operator =(double value)
+    {
+      vector(0) = value;
+      vector(1) = value;
+
+      return *this;
+    }
+
+    //////////////////////////////////////////////////
+    Vector2d Vector2d::operator+(const Vector2d &pt) const
+    {
+      return Vector2d(vector(0) + pt.vector(0), vector(1) + pt.vector(1));
+    }
+
+    const Vector2d &Vector2d::operator+=(const Vector2d &pt)
+    {
+      vector(0) += pt.vector(0);
+      vector(1) += pt.vector(1);
+
+      return *this;
+    }
+
+
+}

Added: trunk/src/libs/geometry/math/vector2d.h
===================================================================
--- trunk/src/libs/geometry/math/vector2d.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/vector2d.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,53 @@
+#ifndef VECTOR2D_H
+#define VECTOR2D_H
+
+#include <eigen3/Eigen/Dense>
+
+namespace math
+{
+    class Vector2d
+    {
+    public:
+        Vector2d();
+        Vector2d(float _x, float _y);
+
+    public: float getX();
+    public: float getY();
+
+    public: float getX() const;
+    public: float getY() const;
+
+        public: void setX(float _x);
+        public: void setY(float _y);
+
+    public:     double mag_squared() const;
+    public:     double mag_squared() ;
+
+
+        //OPERATORS
+        public:    Vector2d operator-(const Vector2d &pt) const;
+        public:    const Vector2d &operator-=(const Vector2d &pt);
+
+        public:    const Vector2d &operator/=(double v);
+        public:    const Vector2d operator/(double v) const;
+        public:    const Vector2d &operator/=(const Vector2d &pt);
+        public:    const Vector2d operator/(const Vector2d &pt) const;
+
+        const Vector2d operator*(const Vector2d &pt) const;
+        const Vector2d &operator*=(const Vector2d &pt);
+        const Vector2d operator*(double v) const;
+        const Vector2d &operator*=(double v);
+
+
+        const Vector2d &operator =(double value);
+        Vector2d &operator =(const Vector2d &pt);
+
+        const Vector2d &operator+=(const Vector2d &pt);
+        Vector2d operator+(const Vector2d &pt) const;
+
+
+    public: Eigen::Vector2f vector;
+    };
+}
+
+#endif // VECTOR2D_H

Added: trunk/src/libs/geometry/math/vector3.cpp
===================================================================
--- trunk/src/libs/geometry/math/vector3.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/vector3.cpp	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,86 @@
+#include "vector3.h"
+namespace math
+{
+    Vector3::Vector3()
+    {
+        vector<< 0.0, 0.0, 0.0;
+    }
+    Vector3::Vector3(const double &_x, const double &_y, const double &_z)
+    {
+        vector<< _x, _y, _z;
+
+    }
+
+    float Vector3::getX()
+    {
+        return vector(0);
+    }
+
+    float Vector3::getY()
+    {
+        return vector(1);
+    }
+    float Vector3::getZ()
+    {
+        return vector(2);
+    }
+
+    void Vector3::setX(float f)
+    {
+        vector(0) = f;
+    }
+
+    void Vector3::setY(float f)
+    {
+        vector(1) = f;
+    }
+
+    void Vector3::setZ(float f)
+    {
+        vector(2) = f;
+    }
+
+    //////////////////////////////////////////////////
+    Vector3 Vector3::normalize()
+    {
+        double d = sqrt(this->getX() * this->getX() + this->getY() * this->getY() + this->getZ() * this->getZ());
+
+      if (!math::equal(d, 0.0))
+      {
+          this->setX(this->getX()/ d);
+          this->setY(this->getY()/ d);
+          this->setZ(this->getZ()/ d);
+      }
+
+      return *this;
+    }
+
+    //////////////////////////////////////////////////
+    Vector3 Vector3::operator+(const Vector3 &pt) const
+    {
+        return Vector3(this->vector(0) + pt.vector(0), this->vector(1) + pt.vector(1), this->vector(2) + pt.vector(2));
+    }
+
+    Vector3 Vector3::operator-(const Vector3 &pt) const
+    {
+        return Vector3(this->vector(0) - pt.vector(0), this->vector(1) - pt.vector(1), this->vector(2) - pt.vector(2));
+    }
+
+    //////////////////////////////////////////////////
+    Vector3 Vector3::operator*(double v) const
+    {
+      return Vector3(this->vector(0) * v, this->vector(1) * v, this->vector(2) * v);
+    }
+
+    //////////////////////////////////////////////////
+    const Vector3 &Vector3::operator+=(const Vector3 &pt)
+    {
+        this->vector(0) += pt.vector(0);
+        this->vector(1) += pt.vector(1);
+        this->vector(2) += pt.vector(2);
+
+      return *this;
+    }
+
+
+}

Added: trunk/src/libs/geometry/math/vector3.h
===================================================================
--- trunk/src/libs/geometry/math/vector3.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/vector3.h	2013-06-05 10:26:11 UTC (rev 927)
@@ -0,0 +1,62 @@
+#ifndef VECTOR3_H
+#define VECTOR3_H
+
+#include <eigen3/Eigen/Dense>
+
+#include "matriz3x3.h"
+
+#include "utils.h"
+
+namespace math
+{
+    class Vector3
+    {
+        public: Vector3();
+        public: Vector3(const double &_x, const double &_y, const double &_z);
+
+        public: float getX();
+        public: float getY();
+        public: float getZ();
+
+        public: void setX(float f);
+        public: void setY(float f);
+        public: void setZ(float f);
+
+        Vector3 normalize();
+        public: Eigen::Vector3f vector;
+
+    public: Vector3 operator+(const Vector3 &pt) const;
+            Vector3 operator-(const Vector3 &pt) const;
+            Vector3 operator*(double v) const;
+            const Vector3 &operator+=(const Vector3 &pt);
+
+
+    public: friend std::ostream &operator<<(std::ostream &_out,
+                                            Vector3 &_pt)
+    {
+            _out << math::precision(_pt.getX(), 6) << " " << math::precision(_pt.getY(), 6) << " "
+            << math::precision(_pt.getZ(), 6);
+            return _out;
+    }
+
+    public: friend std::istream &operator>>(std::istream &_in,
+                                            Vector3 &_pt)
+    {
+      // Skip white spaces
+      _in.setf(std::ios_base::skipws);
+
+      float x, y, z;
+
+      _in >> x >> y >> z;
+      _pt.setX(x);
+      _pt.setY(y);
+      _pt.setZ(z);
+      return _in;
+    }
+
+
+
+    };
+}
+
+#endif // VECTOR3_H



More information about the Jderobot-admin mailing list