[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