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