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

color changes to collada_parser for rviz bug #5237 (by Kei Okada)

This commit is contained in:
rdiankov 2011-12-12 14:55:40 +09:00
parent 1fd9899077
commit 3d58f15f26
1 changed files with 2028 additions and 2049 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

227
collada_parser/src/collada_parser.cpp
View File

@ -1,36 +1,36 @@
/********************************************************************* /*********************************************************************
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* Copyright (c) 2010, University of Tokyo * Copyright (c) 2010, University of Tokyo
* All rights reserved. * All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
* are met: * are met:
* *
* * Redstributions of source code must retain the above copyright * * Redstributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. * notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above * * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following * copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided * disclaimer in the documentation and/or other materials provided
* with the distribution. * with the distribution.
* * Neither the name of the Willow Garage nor the names of its * * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived * contributors may be used to endorse or promote products derived
* from this software without specific prior written permission. * from this software without specific prior written permission.
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/ *********************************************************************/
/* Author: Rosen Diankov, used OpenRAVE files for reference */ /* Author: Rosen Diankov, used OpenRAVE files for reference */
#include <vector> #include <vector>
@ -67,27 +67,30 @@
#include <fcntl.h> #include <fcntl.h>
#endif #endif
#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++) #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++)
#define FOREACHC FOREACH #define FOREACHC FOREACH
namespace urdf{ namespace urdf {
class UnlinkFilename class UnlinkFilename
{ {
public: public:
UnlinkFilename(const std::string& filename) : _filename(filename) {} UnlinkFilename(const std::string& filename) : _filename(filename) {
virtual ~UnlinkFilename() { unlink(_filename.c_str()); } }
virtual ~UnlinkFilename() {
unlink(_filename.c_str());
}
std::string _filename; std::string _filename;
}; };
static std::list<boost::shared_ptr<UnlinkFilename> > _listTempFilenames; static std::list<boost::shared_ptr<UnlinkFilename> > _listTempFilenames;
class ColladaModelReader : public daeErrorHandler class ColladaModelReader : public daeErrorHandler
{ {
class JointAxisBinding class JointAxisBinding
{ {
public: public:
JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) : pvisualtrans(pvisualtrans), pkinematicaxis(pkinematicaxis), jointvalue(jointvalue), kinematics_axis_info(kinematics_axis_info), motion_axis_info(motion_axis_info) { JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) : pvisualtrans(pvisualtrans), pkinematicaxis(pkinematicaxis), jointvalue(jointvalue), kinematics_axis_info(kinematics_axis_info), motion_axis_info(motion_axis_info) {
BOOST_ASSERT( !!pkinematicaxis ); BOOST_ASSERT( !!pkinematicaxis );
daeElement* pae = pvisualtrans->getParentElement(); daeElement* pae = pvisualtrans->getParentElement();
@ -115,7 +118,7 @@ namespace urdf{
/// \brief inter-collada bindings for a kinematics scene /// \brief inter-collada bindings for a kinematics scene
class KinematicsSceneBindings class KinematicsSceneBindings
{ {
public: public:
std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> > listKinematicsVisualBindings; std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> > listKinematicsVisualBindings;
std::list<JointAxisBinding> listAxisBindings; std::list<JointAxisBinding> listAxisBindings;
@ -153,8 +156,10 @@ namespace urdf{
struct USERDATA struct USERDATA
{ {
USERDATA() {} USERDATA() {
USERDATA(double scale) : scale(scale) {} }
USERDATA(double scale) : scale(scale) {
}
double scale; double scale;
boost::shared_ptr<void> p; ///< custom managed data boost::shared_ptr<void> p; ///< custom managed data
}; };
@ -174,7 +179,7 @@ namespace urdf{
///< for sphere it is radius ///< for sphere it is radius
///< for cylinder, first 2 values are radius and height ///< for cylinder, first 2 values are radius and height
///< for trimesh, none ///< for trimesh, none
Color emissionColor, diffuseColor, ambientColor, specularColor; ///< hints for how to color the meshes Color diffuseColor, ambientColor; ///< hints for how to color the meshes
std::vector<Vector3> vertices; std::vector<Vector3> vertices;
std::vector<int> indices; std::vector<int> indices;
@ -388,7 +393,7 @@ namespace urdf{
} }
}; };
public: public:
ColladaModelReader(boost::shared_ptr<ModelInterface> model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) { ColladaModelReader(boost::shared_ptr<ModelInterface> model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
daeErrorHandler::setErrorHandler(this); daeErrorHandler::setErrorHandler(this);
_resourcedir = "."; _resourcedir = ".";
@ -436,7 +441,7 @@ namespace urdf{
return _Extract(); return _Extract();
} }
protected: protected:
/// \extract the first possible robot in the scene /// \extract the first possible robot in the scene
bool _Extract() bool _Extract()
@ -974,7 +979,7 @@ namespace urdf{
} }
else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits
kinematics_limits = true; kinematics_limits = true;
double fscale = (pjoint->type == Joint::REVOLUTE)?(M_PI/180.0f):_GetUnitScale(kinematics_axis_info); double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(kinematics_axis_info);
if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) { if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) {
pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info)); pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info)); pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
@ -1002,7 +1007,7 @@ namespace urdf{
} }
else { else {
ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name)); ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name));
double fscale = (pjoint->type == Joint::REVOLUTE)?(M_PI/180.0f):_GetUnitScale(pdomaxis); double fscale = (pjoint->type == Joint::REVOLUTE) ? (M_PI/180.0f) : _GetUnitScale(pdomaxis);
pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale; pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale;
pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale; pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale;
if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) { if ( pjoint->limits->lower == 0 && pjoint->limits->upper == 0 ) {
@ -1041,17 +1046,13 @@ namespace urdf{
std::vector<std::vector<Vector3> > vertices; std::vector<std::vector<Vector3> > vertices;
std::vector<std::vector<int> > indices; std::vector<std::vector<int> > indices;
std::vector<Color> emission;
std::vector<Color> ambients; std::vector<Color> ambients;
std::vector<Color> diffuses; std::vector<Color> diffuses;
std::vector<Color> specular;
unsigned int index; unsigned int index;
vertices.resize(listGeomProperties.size()); vertices.resize(listGeomProperties.size());
indices.resize(listGeomProperties.size()); indices.resize(listGeomProperties.size());
emission.resize(listGeomProperties.size());
ambients.resize(listGeomProperties.size()); ambients.resize(listGeomProperties.size());
diffuses.resize(listGeomProperties.size()); diffuses.resize(listGeomProperties.size());
specular.resize(listGeomProperties.size());
index = 0; index = 0;
FOREACHC(it, listGeomProperties) { FOREACHC(it, listGeomProperties) {
vertices[index].resize(it->vertices.size()); vertices[index].resize(it->vertices.size());
@ -1062,10 +1063,20 @@ namespace urdf{
for(size_t i = 0; i < it->indices.size(); ++i) { for(size_t i = 0; i < it->indices.size(); ++i) {
indices[index][i] = it->indices[i]; indices[index][i] = it->indices[i];
} }
emission[index] = it->emissionColor;
ambients[index] = it->ambientColor; ambients[index] = it->ambientColor;
diffuses[index] = it->diffuseColor; diffuses[index] = it->diffuseColor;
specular[index] = it->specularColor; // workaround for mesh_loader.cpp:421
// Most of are DAE files don't have ambient color defined
ambients[index].r *= 0.5; ambients[index].g *= 0.5; ambients[index].b *= 0.5;
if ( ambients[index].r == 0.0 ) {
ambients[index].r = 0.0001;
}
if ( ambients[index].g == 0.0 ) {
ambients[index].g = 0.0001;
}
if ( ambients[index].b == 0.0 ) {
ambients[index].b = 0.0001;
}
index++; index++;
} }
@ -1084,7 +1095,7 @@ namespace urdf{
<up_axis>Z_UP</up_axis>\n\ <up_axis>Z_UP</up_axis>\n\
</asset>\n\ </asset>\n\
<library_materials>\n")); <library_materials>\n"));
for(unsigned int i=0; i < index; i++){ for(unsigned int i=0; i < index; i++) {
daedata << str(boost::format("\ daedata << str(boost::format("\
<material id=\"blinn2%d\" name=\"blinn2%d\">\n\ <material id=\"blinn2%d\" name=\"blinn2%d\">\n\
<instance_effect url=\"#blinn2-fx%d\"/>\n\ <instance_effect url=\"#blinn2-fx%d\"/>\n\
@ -1093,15 +1104,12 @@ namespace urdf{
daedata << "\ daedata << "\
</library_materials>\n\ </library_materials>\n\
<library_effects>\n"; <library_effects>\n";
for(unsigned int i=0; i < index; i++){ for(unsigned int i=0; i < index; i++) {
daedata << str(boost::format("\ daedata << str(boost::format("\
<effect id=\"blinn2-fx%d\">\n\ <effect id=\"blinn2-fx%d\">\n\
<profile_COMMON>\n\ <profile_COMMON>\n\
<technique sid=\"common\">\n\ <technique sid=\"common\">\n\
<phong>\n\ <phong>\n\
<emission>\n\
<color>%f %f %f %f</color>\n\
</emission>\n\
<ambient>\n\ <ambient>\n\
<color>%f %f %f %f</color>\n\ <color>%f %f %f %f</color>\n\
</ambient>\n\ </ambient>\n\
@ -1109,33 +1117,18 @@ namespace urdf{
<color>%f %f %f %f</color>\n\ <color>%f %f %f %f</color>\n\
</diffuse>\n\ </diffuse>\n\
<specular>\n\ <specular>\n\
<color>%f %f %f %f</color>\n\ <color>0 0 0 1</color>\n\
</specular>\n\ </specular>\n\
</phong>\n\ </phong>\n\
</technique>\n\ </technique>\n\
</profile_COMMON>\n\ </profile_COMMON>\n\
</effect>\n")%i%emission[i].r%emission[i].g%emission[i].b%emission[i].a%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a%specular[i].r%specular[i].g%specular[i].b%specular[i].a); </effect>\n")%i%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a);
std::cout << str(boost::format("\
%d\n\
<emission>\n\
<color>%f %f %f %f</color>\n\
</emission>\n\
<ambient>\n\
<color>%f %f %f %f</color>\n\
</ambient>\n\
<diffuse>\n\
<color>%f %f %f %f</color>\n\
</diffuse>\n\
<specular>\n\
<color>%f %f %f %f</color>\n\
</specular>\n\
")%i%emission[i].r%emission[i].g%emission[i].b%emission[i].a%ambients[i].r%ambients[i].g%ambients[i].b%ambients[i].a%diffuses[i].r%diffuses[i].g%diffuses[i].b%diffuses[i].a%specular[i].r%specular[i].g%specular[i].b%specular[i].a);
} }
daedata << str(boost::format("\ daedata << str(boost::format("\
</library_effects>\n\ </library_effects>\n\
<library_geometries>\n")); <library_geometries>\n"));
// fill with vertices // fill with vertices
for(unsigned int i=0; i < index; i++){ for(unsigned int i=0; i < index; i++) {
daedata << str(boost::format("\ daedata << str(boost::format("\
<geometry id=\"base2_M1KShape%d\" name=\"base2_M1KShape%d\">\n\ <geometry id=\"base2_M1KShape%d\" name=\"base2_M1KShape%d\">\n\
<mesh>\n\ <mesh>\n\
@ -1174,7 +1167,7 @@ namespace urdf{
<library_visual_scenes>\n\ <library_visual_scenes>\n\
<visual_scene id=\"VisualSceneNode\" name=\"base1d_med\">\n\ <visual_scene id=\"VisualSceneNode\" name=\"base1d_med\">\n\
<node id=\"%s\" name=\"%s\" type=\"NODE\">\n")%name%name); <node id=\"%s\" name=\"%s\" type=\"NODE\">\n")%name%name);
for(unsigned int i=0; i < index; i++){ for(unsigned int i=0; i < index; i++) {
daedata << str(boost::format("\ daedata << str(boost::format("\
<instance_geometry url=\"#base2_M1KShape%i\">\n\ <instance_geometry url=\"#base2_M1KShape%i\">\n\
<bind_material>\n\ <bind_material>\n\
@ -1240,7 +1233,7 @@ namespace urdf{
bool contains=false; bool contains=false;
FOREACHC(it,listAxisBindings) { FOREACHC(it,listAxisBindings) {
// don't check ID's check if the reference is the same! // don't check ID's check if the reference is the same!
if ( (pdomnode->getNode_array()[i]) == (it->visualnode)){ if ( (pdomnode->getNode_array()[i]) == (it->visualnode)) {
contains=true; contains=true;
break; break;
} }
@ -1337,13 +1330,6 @@ namespace urdf{
if( !!peffect ) { if( !!peffect ) {
domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID()))); domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
if( !!pphong ) { if( !!pphong ) {
if( !!pphong->getEmission() && !!pphong->getEmission()->getColor() ) {
domFx_color c = pphong->getEmission()->getColor()->getValue();
geom.emissionColor.r = c[0];
geom.emissionColor.g = c[1];
geom.emissionColor.b = c[2];
geom.emissionColor.a = c[3];
}
if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) { if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
domFx_color c = pphong->getAmbient()->getColor()->getValue(); domFx_color c = pphong->getAmbient()->getColor()->getValue();
geom.ambientColor.r = c[0]; geom.ambientColor.r = c[0];
@ -1358,13 +1344,6 @@ namespace urdf{
geom.diffuseColor.b = c[2]; geom.diffuseColor.b = c[2];
geom.diffuseColor.a = c[3]; geom.diffuseColor.a = c[3];
} }
if( !!pphong->getSpecular() && !!pphong->getSpecular()->getColor() ) {
domFx_color c = pphong->getSpecular()->getColor()->getValue();
geom.specularColor.r = c[0];
geom.specularColor.g = c[1];
geom.specularColor.b = c[2];
geom.specularColor.a = c[3];
}
} }
} }
} }
@ -1395,7 +1374,7 @@ namespace urdf{
size_t triangleIndexStride = 0, vertexoffset = -1; size_t triangleIndexStride = 0, vertexoffset = -1;
domInput_local_offsetRef indexOffsetRef; domInput_local_offsetRef indexOffsetRef;
for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) { for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
size_t offset = triRef->getInput_array()[w]->getOffset(); size_t offset = triRef->getInput_array()[w]->getOffset();
daeString str = triRef->getInput_array()[w]->getSemantic(); daeString str = triRef->getInput_array()[w]->getSemantic();
if (!strcmp(str,"VERTEX")) { if (!strcmp(str,"VERTEX")) {
@ -1411,7 +1390,7 @@ namespace urdf{
const domList_of_uints& indexArray =triRef->getP()->getValue(); const domList_of_uints& indexArray =triRef->getP()->getValue();
geom.indices.reserve(triRef->getCount()*3); geom.indices.reserve(triRef->getCount()*3);
geom.vertices.reserve(triRef->getCount()*3); geom.vertices.reserve(triRef->getCount()*3);
for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) { for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
domInput_localRef localRef = vertsRef->getInput_array()[i]; domInput_localRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic(); daeString str = localRef->getSemantic();
if ( strcmp(str,"POSITION") == 0 ) { if ( strcmp(str,"POSITION") == 0 ) {
@ -1424,10 +1403,10 @@ namespace urdf{
if (!!flArray) { if (!!flArray) {
const domList_of_floats& listFloats = flArray->getValue(); const domList_of_floats& listFloats = flArray->getValue();
int k=vertexoffset; int k=vertexoffset;
int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
for(size_t itri = 0; itri < triRef->getCount(); ++itri) { for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
if(k+2*triangleIndexStride < indexArray.getCount() ) { if(k+2*triangleIndexStride < indexArray.getCount() ) {
for (int j=0;j<3;j++) { for (int j=0; j<3; j++) {
int index0 = indexArray.get(k)*vertexStride; int index0 = indexArray.get(k)*vertexStride;
domFloat fl0 = listFloats.get(index0); domFloat fl0 = listFloats.get(index0);
domFloat fl1 = listFloats.get(index0+1); domFloat fl1 = listFloats.get(index0+1);
@ -1476,7 +1455,7 @@ namespace urdf{
size_t triangleIndexStride = 0, vertexoffset = -1; size_t triangleIndexStride = 0, vertexoffset = -1;
domInput_local_offsetRef indexOffsetRef; domInput_local_offsetRef indexOffsetRef;
for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) { for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
size_t offset = triRef->getInput_array()[w]->getOffset(); size_t offset = triRef->getInput_array()[w]->getOffset();
daeString str = triRef->getInput_array()[w]->getSemantic(); daeString str = triRef->getInput_array()[w]->getSemantic();
if (!strcmp(str,"VERTEX")) { if (!strcmp(str,"VERTEX")) {
@ -1495,7 +1474,7 @@ namespace urdf{
} }
for(size_t ip = 0; ip < primitivecount; ++ip) { for(size_t ip = 0; ip < primitivecount; ++ip) {
domList_of_uints indexArray =triRef->getP_array()[ip]->getValue(); domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) { for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
domInput_localRef localRef = vertsRef->getInput_array()[i]; domInput_localRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic(); daeString str = localRef->getSemantic();
if ( strcmp(str,"POSITION") == 0 ) { if ( strcmp(str,"POSITION") == 0 ) {
@ -1508,7 +1487,7 @@ namespace urdf{
if (!!flArray) { if (!!flArray) {
const domList_of_floats& listFloats = flArray->getValue(); const domList_of_floats& listFloats = flArray->getValue();
int k=vertexoffset; int k=vertexoffset;
int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
size_t usedindices = 3*(indexArray.getCount()-2); size_t usedindices = 3*(indexArray.getCount()-2);
if( geom.indices.capacity() < geom.indices.size()+usedindices ) { if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
geom.indices.reserve(geom.indices.size()+usedindices); geom.indices.reserve(geom.indices.size()+usedindices);
@ -1566,7 +1545,7 @@ namespace urdf{
size_t triangleIndexStride = 0, vertexoffset = -1; size_t triangleIndexStride = 0, vertexoffset = -1;
domInput_local_offsetRef indexOffsetRef; domInput_local_offsetRef indexOffsetRef;
for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) { for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
size_t offset = triRef->getInput_array()[w]->getOffset(); size_t offset = triRef->getInput_array()[w]->getOffset();
daeString str = triRef->getInput_array()[w]->getSemantic(); daeString str = triRef->getInput_array()[w]->getSemantic();
if (!strcmp(str,"VERTEX")) { if (!strcmp(str,"VERTEX")) {
@ -1585,7 +1564,7 @@ namespace urdf{
} }
for(size_t ip = 0; ip < primitivecount; ++ip) { for(size_t ip = 0; ip < primitivecount; ++ip) {
domList_of_uints indexArray = triRef->getP_array()[ip]->getValue(); domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) { for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
domInput_localRef localRef = vertsRef->getInput_array()[i]; domInput_localRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic(); daeString str = localRef->getSemantic();
if ( strcmp(str,"POSITION") == 0 ) { if ( strcmp(str,"POSITION") == 0 ) {
@ -1598,7 +1577,7 @@ namespace urdf{
if (!!flArray) { if (!!flArray) {
const domList_of_floats& listFloats = flArray->getValue(); const domList_of_floats& listFloats = flArray->getValue();
int k=vertexoffset; int k=vertexoffset;
int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
size_t usedindices = indexArray.getCount()-2; size_t usedindices = indexArray.getCount()-2;
if( geom.indices.capacity() < geom.indices.size()+usedindices ) { if( geom.indices.capacity() < geom.indices.size()+usedindices ) {
geom.indices.reserve(geom.indices.size()+usedindices); geom.indices.reserve(geom.indices.size()+usedindices);
@ -1660,7 +1639,7 @@ namespace urdf{
size_t triangleIndexStride = 0,vertexoffset = -1; size_t triangleIndexStride = 0,vertexoffset = -1;
domInput_local_offsetRef indexOffsetRef; domInput_local_offsetRef indexOffsetRef;
for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) { for (unsigned int w=0; w<triRef->getInput_array().getCount(); w++) {
size_t offset = triRef->getInput_array()[w]->getOffset(); size_t offset = triRef->getInput_array()[w]->getOffset();
daeString str = triRef->getInput_array()[w]->getSemantic(); daeString str = triRef->getInput_array()[w]->getSemantic();
if (!strcmp(str,"VERTEX")) { if (!strcmp(str,"VERTEX")) {
@ -1673,7 +1652,7 @@ namespace urdf{
} }
triangleIndexStride++; triangleIndexStride++;
const domList_of_uints& indexArray =triRef->getP()->getValue(); const domList_of_uints& indexArray =triRef->getP()->getValue();
for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) { for (size_t i=0; i<vertsRef->getInput_array().getCount(); ++i) {
domInput_localRef localRef = vertsRef->getInput_array()[i]; domInput_localRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic(); daeString str = localRef->getSemantic();
if ( strcmp(str,"POSITION") == 0 ) { if ( strcmp(str,"POSITION") == 0 ) {
@ -1686,12 +1665,12 @@ namespace urdf{
if (!!flArray) { if (!!flArray) {
const domList_of_floats& listFloats = flArray->getValue(); const domList_of_floats& listFloats = flArray->getValue();
size_t k=vertexoffset; size_t k=vertexoffset;
int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' int vertexStride = 3; //instead of hardcoded stride, should use the 'accessor'
for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) { for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
size_t numverts = triRef->getVcount()->getValue()[ipoly]; size_t numverts = triRef->getVcount()->getValue()[ipoly];
if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) { if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
size_t startoffset = geom.vertices.size(); size_t startoffset = geom.vertices.size();
for (size_t j=0;j<numverts;j++) { for (size_t j=0; j<numverts; j++) {
int index0 = indexArray.get(k)*vertexStride; int index0 = indexArray.get(k)*vertexStride;
domFloat fl0 = listFloats.get(index0); domFloat fl0 = listFloats.get(index0);
domFloat fl1 = listFloats.get(index0+1); domFloat fl1 = listFloats.get(index0+1);
@ -1728,16 +1707,16 @@ namespace urdf{
std::vector<Vector3> vconvexhull; std::vector<Vector3> vconvexhull;
if (geom->getMesh()) { if (geom->getMesh()) {
const domMeshRef meshRef = geom->getMesh(); const domMeshRef meshRef = geom->getMesh();
for (size_t tg = 0;tg<meshRef->getTriangles_array().getCount();tg++) { for (size_t tg = 0; tg<meshRef->getTriangles_array().getCount(); tg++) {
_ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
} }
for (size_t tg = 0;tg<meshRef->getTrifans_array().getCount();tg++) { for (size_t tg = 0; tg<meshRef->getTrifans_array().getCount(); tg++) {
_ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
} }
for (size_t tg = 0;tg<meshRef->getTristrips_array().getCount();tg++) { for (size_t tg = 0; tg<meshRef->getTristrips_array().getCount(); tg++) {
_ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
} }
for (size_t tg = 0;tg<meshRef->getPolylist_array().getCount();tg++) { for (size_t tg = 0; tg<meshRef->getPolylist_array().getCount(); tg++) {
_ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties);
} }
if( meshRef->getPolygons_array().getCount()> 0 ) { if( meshRef->getPolygons_array().getCount()> 0 ) {
@ -1802,10 +1781,10 @@ namespace urdf{
domGeometryRef lib = libgeom->getGeometry_array()[i]; domGeometryRef lib = libgeom->getGeometry_array()[i];
if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2 if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2
//found convex_hull geometry //found convex_hull geometry
domMesh *meshElement = lib->getMesh();//linkedGeom->getMesh(); domMesh *meshElement = lib->getMesh(); //linkedGeom->getMesh();
if (meshElement) { if (meshElement) {
const domVerticesRef vertsRef = meshElement->getVertices(); const domVerticesRef vertsRef = meshElement->getVertices();
for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) { for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
domInput_localRef localRef = vertsRef->getInput_array()[i]; domInput_localRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic(); daeString str = localRef->getSemantic();
if ( strcmp(str,"POSITION") == 0) { if ( strcmp(str,"POSITION") == 0) {
@ -1818,7 +1797,7 @@ namespace urdf{
if (!!flArray) { if (!!flArray) {
vconvexhull.reserve(vconvexhull.size()+flArray->getCount()); vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
const domList_of_floats& listFloats = flArray->getValue(); const domList_of_floats& listFloats = flArray->getValue();
for (size_t k=0;k+2<flArray->getCount();k+=3) { for (size_t k=0; k+2<flArray->getCount(); k+=3) {
domFloat fl0 = listFloats.get(k); domFloat fl0 = listFloats.get(k);
domFloat fl1 = listFloats.get(k+1); domFloat fl1 = listFloats.get(k+1);
domFloat fl2 = listFloats.get(k+2); domFloat fl2 = listFloats.get(k+2);
@ -1835,7 +1814,7 @@ namespace urdf{
else { else {
//no getConvex_hull_of but direct vertices //no getConvex_hull_of but direct vertices
const domVerticesRef vertsRef = convexRef->getVertices(); const domVerticesRef vertsRef = convexRef->getVertices();
for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) { for (size_t i=0; i<vertsRef->getInput_array().getCount(); i++) {
domInput_localRef localRef = vertsRef->getInput_array()[i]; domInput_localRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic(); daeString str = localRef->getSemantic();
if ( strcmp(str,"POSITION") == 0 ) { if ( strcmp(str,"POSITION") == 0 ) {
@ -1848,7 +1827,7 @@ namespace urdf{
if (!!flArray) { if (!!flArray) {
const domList_of_floats& listFloats = flArray->getValue(); const domList_of_floats& listFloats = flArray->getValue();
vconvexhull.reserve(vconvexhull.size()+flArray->getCount()); vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
for (size_t k=0;k+2<flArray->getCount();k+=3) { for (size_t k=0; k+2<flArray->getCount(); k+=3) {
domFloat fl0 = listFloats.get(k); domFloat fl0 = listFloats.get(k);
domFloat fl1 = listFloats.get(k+1); domFloat fl1 = listFloats.get(k+1);
domFloat fl2 = listFloats.get(k+2); domFloat fl2 = listFloats.get(k+2);
@ -2596,18 +2575,18 @@ namespace urdf{
std::string _resourcedir; std::string _resourcedir;
boost::shared_ptr<ModelInterface> _model; boost::shared_ptr<ModelInterface> _model;
}; };
boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str) boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str)
{ {
boost::shared_ptr<ModelInterface> model(new ModelInterface); boost::shared_ptr<ModelInterface> model(new ModelInterface);
ColladaModelReader reader(model); ColladaModelReader reader(model);
if (!reader.InitFromData(xml_str)) if (!reader.InitFromData(xml_str))
model.reset(); model.reset();
return model; return model;
} }
} }