bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada_urdf: library_geometries finished

This commit is contained in:
tfield 2010-03-05 07:09:36 +00:00
parent 1a48be8bb4
commit 9ef55ab12d
3 changed files with 174 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
collada_urdf/src/STLLoader.cpp
View File

@ -54,33 +54,17 @@ bool Vector3::operator==(const Vector3& v) const {
Mesh::Mesh() { Mesh::Mesh() {
} }
bool Mesh::hasVertex(const Vector3& v) const { int Mesh::getVertexIndex(const Vector3& v) const {
for (std::vector<Vector3>::const_iterator i = vertices.begin(); i != vertices.end(); i++)
if (v == *i)
return true;
return false;
}
unsigned int Mesh::getVertexIndex(const Vector3& v) const {
for (unsigned int i = 0; i < vertices.size(); i++) for (unsigned int i = 0; i < vertices.size(); i++)
if (vertices[i] == v) if (vertices[i] == v)
return true; return i;
return false; return -1;
} }
void Mesh::addVertex(const Vector3& v) { void Mesh::addVertex(const Vector3& v) { vertices.push_back(v); }
vertices.push_back(v); void Mesh::addNormal(const Vector3& n) { normals.push_back(n); }
} void Mesh::addIndex(unsigned int i) { indices.push_back(i); }
void Mesh::addNormal(const Vector3& n) {
normals.push_back(n);
}
void Mesh::addIndex(unsigned int i) {
indices.push_back(i);
}
// //
@ -104,12 +88,14 @@ void STLLoader::readBinary(FILE* filein, Mesh* mesh) {
for (int iface = 0; iface < face_num; iface++) { for (int iface = 0; iface < face_num; iface++) {
Vector3 normal(readFloat(filein), readFloat(filein), readFloat(filein)); Vector3 normal(readFloat(filein), readFloat(filein), readFloat(filein));
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
Vector3 vertex(readFloat(filein), readFloat(filein), readFloat(filein)); Vector3 vertex(readFloat(filein), readFloat(filein), readFloat(filein));
if (!mesh->hasVertex(vertex)) { int index = mesh->getVertexIndex(vertex);
if (index == -1) {
mesh->addVertex(vertex); mesh->addVertex(vertex);
mesh->addNormal(normal); mesh->addNormal(normal);
index = mesh->vertices.size() - 1;
} }
mesh->addIndex(mesh->getVertexIndex(vertex)); mesh->addIndex(index);
} }
readShortInt(filein); // 2 byte attribute readShortInt(filein); // 2 byte attribute
} }

5
collada_urdf/src/STLLoader.h
View File

@ -67,14 +67,13 @@ namespace collada_urdf
public: public:
Mesh(); Mesh();
bool hasVertex(const Vector3& v) const; int getVertexIndex(const Vector3& v) const;
unsigned int getVertexIndex(const Vector3& v) const;
void addVertex(const Vector3& v); void addVertex(const Vector3& v);
void addNormal(const Vector3& n); void addNormal(const Vector3& n);
void addIndex(unsigned int i); void addIndex(unsigned int i);
private: public:
std::vector<Vector3> vertices; std::vector<Vector3> vertices;
std::vector<Vector3> normals; std::vector<Vector3> normals;
std::vector<unsigned int> indices; std::vector<unsigned int> indices;

273
collada_urdf/src/urdf_to_collada.cpp
View File

@ -84,21 +84,19 @@ public:
urdf::Model* robot_; urdf::Model* robot_;
Assimp::Importer importer_;
boost::shared_ptr<DAE> collada_; boost::shared_ptr<DAE> collada_;
domCOLLADA* dom_; domCOLLADA* dom_;
domCOLLADA::domSceneRef scene_; domCOLLADA::domSceneRef scene_;
domLibrary_jointsRef jointsLib_;
domLibrary_geometriesRef geometriesLib_;
domLibrary_visual_scenesRef visualScenesLib_; domLibrary_visual_scenesRef visualScenesLib_;
domLibrary_kinematics_scenesRef kinematicsScenesLib_; domLibrary_kinematics_scenesRef kinematicsScenesLib_;
domLibrary_kinematics_modelsRef kinematicsModelsLib_; domLibrary_kinematics_modelsRef kinematicsModelsLib_;
domLibrary_articulated_systemsRef articulatedSystemsLib_; domLibrary_jointsRef jointsLib_;
domLibrary_physics_scenesRef physicsScenesLib_; domLibrary_physics_scenesRef physicsScenesLib_;
domLibrary_materialsRef materialsLib_; domLibrary_materialsRef materialsLib_;
domLibrary_effectsRef effectsLib_; domLibrary_effectsRef effectsLib_;
domLibrary_geometriesRef geometriesLib_; //domLibrary_articulated_systemsRef articulatedSystemsLib_;
public: public:
ColladaWriter(urdf::Model* robot) : robot_(robot) { ColladaWriter(urdf::Model* robot) : robot_(robot) {
@ -145,23 +143,26 @@ public:
scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->createAndPlace(COLLADA_ELEMENT_SCENE)); scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->createAndPlace(COLLADA_ELEMENT_SCENE));
visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
visualScenesLib_->setId("visual_scenes"); visualScenesLib_->setId("vscenes");
geometriesLib_ = daeSafeCast<domLibrary_geometries>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); geometriesLib_ = daeSafeCast<domLibrary_geometries>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
geometriesLib_->setId("geometries"); geometriesLib_->setId("geometries");
kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
kinematicsScenesLib_->setId("kinematics_scenes");
kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
kinematicsModelsLib_->setId("kinematics_models");
jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS));
jointsLib_->setId("joints");
/*
effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS)); effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS));
effectsLib_->setId("effects"); effectsLib_->setId("effects");
materialsLib_ = daeSafeCast<domLibrary_materials>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_MATERIALS)); materialsLib_ = daeSafeCast<domLibrary_materials>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_MATERIALS));
materialsLib_->setId("materials"); materialsLib_->setId("materials");
kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
kinematicsModelsLib_->setId("kinematics_models");
articulatedSystemsLib_ = daeSafeCast<domLibrary_articulated_systems>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS)); articulatedSystemsLib_ = daeSafeCast<domLibrary_articulated_systems>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
articulatedSystemsLib_->setId("articulated_systems"); articulatedSystemsLib_->setId("articulated_systems");
kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
kinematicsScenesLib_->setId("kinematics_scenes");
physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
physicsScenesLib_->setId("physics_scenes"); physicsScenesLib_->setId("physics_scenes");
jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
jointsLib_->setId("joints"); */
} }
virtual ~ColladaWriter() { virtual ~ColladaWriter() {
@ -174,31 +175,32 @@ public:
// Create visual scene // Create visual scene
s.vscene = daeSafeCast<domVisual_scene>(visualScenesLib_->createAndPlace(COLLADA_ELEMENT_VISUAL_SCENE)); s.vscene = daeSafeCast<domVisual_scene>(visualScenesLib_->createAndPlace(COLLADA_ELEMENT_VISUAL_SCENE));
s.vscene->setId("visual_scene"); s.vscene->setId("vscene");
s.vscene->setName("URDF Visual Scene"); s.vscene->setName("URDF Visual Scene");
// Create instance visual scene
s.viscene = daeSafeCast<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE));
s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str());
// Create kinematics scene // Create kinematics scene
s.kscene = daeSafeCast<domKinematics_scene>(kinematicsScenesLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_SCENE)); s.kscene = daeSafeCast<domKinematics_scene>(kinematicsScenesLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_SCENE));
s.kscene->setId("kinematics_scene"); s.kscene->setId("kinematics_scene");
s.kscene->setName("URDF Kinematics Scene"); s.kscene->setName("URDF Kinematics Scene");
// Create physic scene
s.pscene = daeSafeCast<domPhysics_scene>(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE));
s.pscene->setId("physics_scene");
s.pscene->setName("URDF Physics Scene");
// Create instance visual scene
s.viscene = daeSafeCast<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE));
s.viscene->setUrl((string("#") + string(s.vscene->getID())).c_str());
// Create instance kinematics scene // Create instance kinematics scene
s.kiscene = daeSafeCast<domInstance_kinematics_scene>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE)); s.kiscene = daeSafeCast<domInstance_kinematics_scene>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE));
s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str()); s.kiscene->setUrl((string("#") + string(s.kscene->getID())).c_str());
/*
// Create physics scene
s.pscene = daeSafeCast<domPhysics_scene>(physicsScenesLib_->createAndPlace(COLLADA_ELEMENT_PHYSICS_SCENE));
s.pscene->setId("physics_scene");
s.pscene->setName("URDF Physics Scene");
// Create instance physics scene // Create instance physics scene
s.piscene = daeSafeCast<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE)); s.piscene = daeSafeCast<domInstance_with_extra>(scene_->createAndPlace(COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE));
s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str()); s.piscene->setUrl((string("#") + string(s.pscene->getID())).c_str());
*/
return s; return s;
} }
@ -213,11 +215,13 @@ public:
bool writeScene() { bool writeScene() {
SCENE scene = createScene(); SCENE scene = createScene();
/*
domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// Create gravity // Create gravity
domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->createAndPlace (COLLADA_ELEMENT_GRAVITY)); domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->createAndPlace(COLLADA_ELEMENT_GRAVITY));
g->getValue().set3(0.0, 0.0, 1.0); g->getValue().set3(0.0, 0.0, 1.0);
*/
addJoints(); addJoints();
addKinematics(); addKinematics();
@ -229,6 +233,8 @@ public:
} }
void addGeometries() { void addGeometries() {
int link_num = 0;
for (map<string, boost::shared_ptr<urdf::Link> >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { for (map<string, boost::shared_ptr<urdf::Link> >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) {
boost::shared_ptr<urdf::Link> urdf_link = i->second; boost::shared_ptr<urdf::Link> urdf_link = i->second;
@ -237,14 +243,21 @@ public:
switch (urdf_link->visual->geometry->type) { switch (urdf_link->visual->geometry->type) {
case urdf::Geometry::MESH: { case urdf::Geometry::MESH: {
urdf::Mesh* mesh = (urdf::Mesh*) urdf_link->visual->geometry.get(); urdf::Mesh* urdf_mesh = (urdf::Mesh*) urdf_link->visual->geometry.get();
string filename = mesh->filename; string filename = urdf_mesh->filename;
urdf::Vector3 scale = mesh->scale; urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale
// <geometry id="g1.link0.geom0">
domGeometryRef geometry = daeSafeCast<domGeometry>(geometriesLib_->createAndPlace(COLLADA_ELEMENT_GEOMETRY)); domGeometryRef geometry = daeSafeCast<domGeometry>(geometriesLib_->createAndPlace(COLLADA_ELEMENT_GEOMETRY));
loadMesh(filename, geometry); string geometry_id = string("g1.link") + boost::lexical_cast<string>(link_num) + string(".geom0");
geometry->setId(geometry_id.c_str());
{
loadMesh(filename, geometry, geometry_id);
}
// </geometry>
link_num++;
break; break;
} }
case urdf::Geometry::SPHERE: { case urdf::Geometry::SPHERE: {
@ -267,7 +280,7 @@ public:
} }
} }
void loadMesh(const string& filename, domGeometryRef mesh) { void loadMesh(const string& filename, domGeometryRef geometry, const string& geometry_id) {
// Load the mesh // Load the mesh
resource_retriever::MemoryResource resource; resource_retriever::MemoryResource resource;
resource_retriever::Retriever retriever; resource_retriever::Retriever retriever;
@ -280,22 +293,23 @@ public:
} }
// Try assimp first, then STLLoader // Try assimp first, then STLLoader
if (!loadMeshWithAssimp(resource, mesh)) if (!loadMeshWithAssimp(resource, geometry, geometry_id))
if (!loadMeshWithSTLLoader(resource, mesh)) if (!loadMeshWithSTLLoader(resource, geometry, geometry_id))
cerr << "*** Can't load " << filename << endl; cerr << "*** Can't load " << filename << endl;
} }
bool loadMeshWithAssimp(const resource_retriever::MemoryResource& resource, domGeometryRef mesh) { bool loadMeshWithAssimp(const resource_retriever::MemoryResource& resource, domGeometryRef geometry, const string& geometry_id) {
// Import the mesh using assimp // Import the mesh using assimp
const aiScene* scene = importer_.ReadFileFromMemory(resource.data.get(), resource.size, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */); Assimp::Importer importer;
const aiScene* scene = importer.ReadFileFromMemory(resource.data.get(), resource.size, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */);
if (!scene) if (!scene)
return false; return false;
buildMeshFromAssimp(scene->mRootNode, mesh); buildMeshFromAssimp(scene, scene->mRootNode, geometry, geometry_id);
return true; return true;
} }
bool loadMeshWithSTLLoader(const resource_retriever::MemoryResource& resource, domGeometryRef mesh) { bool loadMeshWithSTLLoader(const resource_retriever::MemoryResource& resource, domGeometryRef geometry, const string& geometry_id) {
// Write the resource to a temporary file // Write the resource to a temporary file
char tmp_filename[] = "/tmp/collada_urdf_XXXXXX"; char tmp_filename[] = "/tmp/collada_urdf_XXXXXX";
int fd = mkstemp(tmp_filename); int fd = mkstemp(tmp_filename);
@ -305,7 +319,7 @@ public:
// Import the mesh using STLLoader // Import the mesh using STLLoader
STLLoader loader; STLLoader loader;
Mesh* stl_mesh = loader.load(string(tmp_filename)); Mesh* stl_mesh = loader.load(string(tmp_filename));
buildMeshFromSTLLoader(stl_mesh, mesh); buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id);
delete stl_mesh; delete stl_mesh;
// Delete the temporary file // Delete the temporary file
@ -314,16 +328,88 @@ public:
return true; return true;
} }
void buildMeshFromSTLLoader(Mesh* mesh, daeElementRef parent) { void buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, const string& geometry_id) {
// // <mesh>
domMeshRef mesh = daeSafeCast<domMesh>(parent->createAndPlace(COLLADA_ELEMENT_MESH));
{
unsigned int num_vertices = stl_mesh->vertices.size();
unsigned int num_indices = stl_mesh->indices.size();
unsigned int num_faces = num_indices / 3;
// <source id="g1.link0.geom0.positions">
domSourceRef positions_source = daeSafeCast<domSource>(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE));
positions_source->setId((geometry_id + string(".positions")).c_str());
{
// <float_array id="g1.link0.geom0.positions-array" count="4533" digits="6">
domFloat_arrayRef positions_array = daeSafeCast<domFloat_array>(positions_source->createAndPlace(COLLADA_ELEMENT_FLOAT_ARRAY));
positions_array->setId((geometry_id + string(".positions-array")).c_str());
positions_array->setCount(num_vertices);
positions_array->setDigits(6); // 6 decimal places
positions_array->getValue().setCount(num_vertices);
for (unsigned int j = 0; j < num_vertices; j++) {
positions_array->getValue()[j] = stl_mesh->vertices[j].x;
positions_array->getValue()[j] = stl_mesh->vertices[j].y;
positions_array->getValue()[j] = stl_mesh->vertices[j].z;
}
// <technique_common>
domSource::domTechnique_commonRef source_tech = daeSafeCast<domSource::domTechnique_common>(positions_source->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// <accessor count="4533" source="#g1.link0.geom0.positions-array" stride="3">
domAccessorRef accessor = daeSafeCast<domAccessor>(source_tech->createAndPlace(COLLADA_ELEMENT_ACCESSOR));
accessor->setCount(num_vertices);
accessor->setSource(xsAnyURI(*positions_array, string("#") + geometry_id + string(".positions-array")));
accessor->setStride(3);
// <param name="X" type="float"/>
// <param name="Y" type="float"/>
// <param name="Z" type="float"/>
domParamRef px = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(accessor->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
}
// <vertices id="vertices">
domVerticesRef vertices = daeSafeCast<domVertices>(mesh->createAndPlace(COLLADA_ELEMENT_VERTICES));
vertices->setId("vertices");
{
// <input semantic="POSITION" source="#g1.link0.geom0.positions"/>
domInput_localRef vertices_input = daeSafeCast<domInput_local>(vertices->createAndPlace(COLLADA_ELEMENT_INPUT));
vertices_input->setSemantic("POSITION");
vertices_input->setSource(domUrifragment(*positions_source, string("#") + geometry_id + string(".positions")));
}
// </vertices>
// <triangles count="1511" material="mat0">
domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES));
{
triangles->setCount(num_faces);
triangles->setMaterial("mat0");
// <input offset="0" semantic="VERTEX" source="#g1.link0.geom0/vertices" set="0"/>
domInput_local_offsetRef vertex_offset = daeSafeCast<domInput_local_offset>(triangles->createAndPlace(COLLADA_ELEMENT_INPUT));
vertex_offset->setSemantic("VERTEX");
vertex_offset->setOffset(0);
vertex_offset->setSource(domUrifragment(*positions_source, string("#") + geometry_id + string("/vertices")));
// <p>0 1 2 3 ...
domPRef indices = daeSafeCast<domP>(triangles->createAndPlace(COLLADA_ELEMENT_P));
indices->getValue().setCount(num_indices);
for (unsigned int i = 0; i < num_indices; i++)
indices->getValue()[i] = stl_mesh->indices[i];
// </p>
}
// </triangles>
}
// </mesh>
} }
void buildMeshFromAssimp(aiNode* node, daeElementRef parent) { // EXPERIMENTAL: untested
void buildMeshFromAssimp(const aiScene* scene, aiNode* node, daeElementRef parent, const string& geometry_id) {
if (node == NULL) if (node == NULL)
return; return;
const aiScene* scene = importer_.GetScene();
aiMatrix4x4 transform = node->mTransformation; aiMatrix4x4 transform = node->mTransformation;
aiNode* pnode = node->mParent; aiNode* pnode = node->mParent;
@ -334,13 +420,12 @@ public:
pnode = pnode->mParent; pnode = pnode->mParent;
} }
string parentid("mesh"); // <mesh>
domMeshRef mesh = daeSafeCast<domMesh>(parent->createAndPlace(COLLADA_ELEMENT_MESH));
for (unsigned int i = 0; i < node->mNumMeshes; i++) { {
aiMesh* aMesh = scene->mMeshes[i]; for (unsigned int i = 0; i < node->mNumMeshes; i++) {
aiMesh* aMesh = scene->mMeshes[i];
domMeshRef mesh = daeSafeCast<domMesh>(parent->createAndPlace(COLLADA_ELEMENT_MESH));
{
// Add in the indices for each face // Add in the indices for each face
for (unsigned int j = 0; j < aMesh->mNumFaces; j++) { for (unsigned int j = 0; j < aMesh->mNumFaces; j++) {
aiFace* aFace = &(aMesh->mFaces[j]); aiFace* aFace = &(aMesh->mFaces[j]);
@ -350,95 +435,59 @@ public:
//subMesh->AddIndex(aFace->mIndices[k]); //subMesh->AddIndex(aFace->mIndices[k]);
} }
} }
domSourceRef positions_source = daeSafeCast<domSource>(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE)); domSourceRef positions_source = daeSafeCast<domSource>(mesh->createAndPlace(COLLADA_ELEMENT_SOURCE));
{ {
positions_source->setId((parentid + string(".positions")).c_str()); positions_source->setId((geometry_id + string(".positions")).c_str());
domFloat_arrayRef positions_array = daeSafeCast<domFloat_array>(positions_source->createAndPlace(COLLADA_ELEMENT_FLOAT_ARRAY)); domFloat_arrayRef positions_array = daeSafeCast<domFloat_array>(positions_source->createAndPlace(COLLADA_ELEMENT_FLOAT_ARRAY));
positions_array->setId((parentid + string(".positions-array")).c_str()); positions_array->setId((geometry_id + string(".positions-array")).c_str());
positions_array->setCount(aMesh->mNumVertices); positions_array->setCount(aMesh->mNumVertices);
positions_array->setDigits(6); // 6 decimal places positions_array->setDigits(6); // 6 decimal places
positions_array->getValue().setCount(3 * aMesh->mNumVertices); positions_array->getValue().setCount(3 * aMesh->mNumVertices);
for (unsigned int j = 0; j < aMesh->mNumVertices; j++) { for (unsigned int j = 0; j < aMesh->mNumVertices; j++) {
aiVector3D p; aiVector3D p;
p.x = aMesh->mVertices[j].x; p.x = aMesh->mVertices[j].x;
p.y = aMesh->mVertices[j].y; p.y = aMesh->mVertices[j].y;
p.z = aMesh->mVertices[j].z; p.z = aMesh->mVertices[j].z;
p *= transform; p *= transform;
positions_array->getValue()[j] = p.x; positions_array->getValue()[j] = p.x;
positions_array->getValue()[j] = p.y; positions_array->getValue()[j] = p.y;
positions_array->getValue()[j] = p.z; positions_array->getValue()[j] = p.z;
/* /*
if (aMesh->HasNormals()) { if (aMesh->HasNormals()) {
p.x = aMesh->mNormals[j].x; p.x = aMesh->mNormals[j].x;
p.y = aMesh->mNormals[j].y; p.y = aMesh->mNormals[j].y;
p.z = aMesh->mNormals[j].z; p.z = aMesh->mNormals[j].z;
} }
// @todo add normal // @todo add normal
//subMesh->AddNormal(p.x, p.y, p.z); //subMesh->AddNormal(p.x, p.y, p.z);
// @todo add tex coord // @todo add tex coord
//if (aMesh->mNumUVComponents[0]) //if (aMesh->mNumUVComponents[0])
// subMesh->AddTexCoord(aMesh->mTextureCoords[0][j].x, 1.0 -aMesh->mTextureCoords[0][j].y); // subMesh->AddTexCoord(aMesh->mTextureCoords[0][j].x, 1.0 -aMesh->mTextureCoords[0][j].y);
//else //else
// subMesh->AddTexCoord(0,0); // subMesh->AddTexCoord(0,0);
*/ */
} }
/*
domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
domAccessorRef pacc = daeSafeCast<domAccessor>(psourcetec->createAndPlace(COLLADA_ELEMENT_ACCESSOR));
pacc->setCount(aMesh->mNumVertices);
pacc->setSource(xsAnyURI(*positions_array, string("#") + parentid + string(".positions-array")));
pacc->setStride(3);
domParamRef px = daeSafeCast<domParam>(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(pacc->createAndPlace(COLLADA_ELEMENT_PARAM)); pz->setName("Z"); pz->setType("float");
*/
} }
domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES)); domTrianglesRef triangles = daeSafeCast<domTriangles>(mesh->createAndPlace(COLLADA_ELEMENT_TRIANGLES));
{ {
triangles->setCount(aMesh->mNumFaces / 3); triangles->setCount(aMesh->mNumFaces / 3);
//triangles->setMaterial("mat0");
/*
domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->createAndPlace(COLLADA_ELEMENT_INPUT));
pvertoffset->setSemantic("VERTEX");
pvertoffset->setOffset(0);
pvertoffset->setSource(domUrifragment(*pverts, string("#") + parentid + string("/vertices")));
domPRef pindices = daeSafeCast<domP>(ptris->createAndPlace(COLLADA_ELEMENT_P));
pindices->getValue().setCount(mesh.indices.size());
for (size_t ind = 0; ind < mesh.indices.size(); ++ind)
pindices->getValue()[ind] = mesh.indices[ind];
*/
} }
} }
for (unsigned int i = 0; i < node->mNumChildren; i++) for (unsigned int i = 0; i < node->mNumChildren; i++)
buildMeshFromAssimp(node->mChildren[i], mesh); buildMeshFromAssimp(scene, node->mChildren[i], mesh, geometry_id);
} }
} }
/*
domGeometryRef writeGeometry(const string& parentid) {
domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->createAndPlace(COLLADA_ELEMENT_VERTICES));
{
pverts->setId("vertices");
domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->createAndPlace(COLLADA_ELEMENT_INPUT));
pvertinput->setSemantic("POSITION");
pvertinput->setSource(domUrifragment(*pvertsource, string("#")+parentid+string(".positions")));
}
}
*/
void addJoints() { void addJoints() {
for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) {
boost::shared_ptr<urdf::Joint> urdf_joint = i->second; boost::shared_ptr<urdf::Joint> urdf_joint = i->second;
@ -500,11 +549,11 @@ public:
break; break;
} }
case urdf::Joint::FIXED: { case urdf::Joint::FIXED: {
// Model as a PRISMATIC joint with no limits // Model as a REVOLUTE joint with no leeway
// joint.axis // joint.axis
vector<domAxis_constraintRef> axes(1); vector<domAxis_constraintRef> axes(1);
axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC)); axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE));
axes[0]->setSid("axis0"); axes[0]->setSid("axis0");
domAxisRef axis = daeSafeCast<domAxis>(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS)); domAxisRef axis = daeSafeCast<domAxis>(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS));
axis->getValue().setCount(3); axis->getValue().setCount(3);