p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/common/ColladaLoader.cc

2069 lines
66 KiB
C++
Raw Permalink Blame History

/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <tinyxml.h>
#include <math.h>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/unordered_map.hpp>
#include "gazebo/math/Helpers.hh"
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Vector2d.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Matrix4.hh"
#include "gazebo/math/Quaternion.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Material.hh"
#include "gazebo/common/Mesh.hh"
#include "gazebo/common/Skeleton.hh"
#include "gazebo/common/SkeletonAnimation.hh"
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/ColladaLoaderPrivate.hh"
#include "gazebo/common/ColladaLoader.hh"
using namespace gazebo;
using namespace common;
/////////////////////////////////////////////////
struct Vector3Hash : std::unary_function<const ignition::math::Vector3d,
std::size_t>
{
std::size_t operator()(const ignition::math::Vector3d &_v) const
{
std::size_t seed = 0;
boost::hash_combine(seed, _v.X());
boost::hash_combine(seed, _v.Y());
boost::hash_combine(seed, _v.Z());
return seed;
}
};
/////////////////////////////////////////////////
struct Vector2dHash : std::unary_function<const ignition::math::Vector2d,
std::size_t>
{
std::size_t operator()(const ignition::math::Vector2d &_v) const
{
std::size_t seed = 0;
boost::hash_combine(seed, _v.X());
boost::hash_combine(seed, _v.Y());
return seed;
}
};
//////////////////////////////////////////////////
ColladaLoader::ColladaLoader()
: MeshLoader(), dataPtr(new ColladaLoaderPrivate)
{
this->dataPtr->meter = 1.0;
}
//////////////////////////////////////////////////
ColladaLoader::~ColladaLoader()
{
delete this->dataPtr;
this->dataPtr = 0;
}
//////////////////////////////////////////////////
Mesh *ColladaLoader::Load(const std::string &_filename)
{
this->dataPtr->positionIds.clear();
this->dataPtr->normalIds.clear();
this->dataPtr->texcoordIds.clear();
this->dataPtr->materialIds.clear();
this->dataPtr->positionDuplicateMap.clear();
this->dataPtr->normalDuplicateMap.clear();
this->dataPtr->texcoordDuplicateMap.clear();
// reset scale
this->dataPtr->meter = 1.0;
TiXmlDocument xmlDoc;
this->dataPtr->path.clear();
if (_filename.rfind('/') != std::string::npos)
{
this->dataPtr->path = _filename.substr(0, _filename.rfind('/'));
}
this->dataPtr->filename = _filename;
if (!xmlDoc.LoadFile(_filename))
gzerr << "Unable to load collada file[" << _filename << "]\n";
this->dataPtr->colladaXml = xmlDoc.FirstChildElement("COLLADA");
if (!this->dataPtr->colladaXml)
gzerr << "Missing COLLADA tag\n";
if (std::string(this->dataPtr->colladaXml->Attribute("version")) != "1.4.0" &&
std::string(this->dataPtr->colladaXml->Attribute("version")) != "1.4.1")
gzerr << "Invalid collada file. Must be version 1.4.0 or 1.4.1\n";
TiXmlElement *assetXml =
this->dataPtr->colladaXml->FirstChildElement("asset");
if (assetXml)
{
TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
if (unitXml && unitXml->Attribute("meter"))
this->dataPtr->meter = ignition::math::parseFloat(
unitXml->Attribute("meter"));
}
Mesh *mesh = new Mesh();
mesh->SetPath(this->dataPtr->path);
this->LoadScene(mesh);
// This will make the model the correct size.
mesh->Scale(this->dataPtr->meter);
return mesh;
}
/////////////////////////////////////////////////
void ColladaLoader::LoadScene(Mesh *_mesh)
{
TiXmlElement *sceneXml =
this->dataPtr->colladaXml->FirstChildElement("scene");
std::string sceneURL =
sceneXml->FirstChildElement("instance_visual_scene")->Attribute("url");
TiXmlElement *visSceneXml = this->GetElementId("visual_scene", sceneURL);
if (!visSceneXml)
{
gzerr << "Unable to find visual_scene id ='" << sceneURL << "'\n";
return;
}
TiXmlElement *nodeXml = visSceneXml->FirstChildElement("node");
while (nodeXml)
{
this->LoadNode(nodeXml, _mesh, ignition::math::Matrix4d::Identity);
nodeXml = nodeXml->NextSiblingElement("node");
}
}
/////////////////////////////////////////////////
void ColladaLoader::LoadNode(TiXmlElement *_elem, Mesh *_mesh,
const ignition::math::Matrix4d &_transform)
{
TiXmlElement *nodeXml;
TiXmlElement *instGeomXml;
ignition::math::Matrix4d transform = this->LoadNodeTransform(_elem);
transform = _transform * transform;
if (_elem->Attribute("name"))
{
this->dataPtr->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)
{
gzerr << "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->dataPtr->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->dataPtr->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->dataPtr->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->dataPtr->materialMap[symbol] = target;
matXml = matXml->NextSiblingElement("instance_material");
}
}
bindMatXml = bindMatXml->NextSiblingElement("bind_material");
}
this->LoadController(contrXml, rootNodeXml, transform, _mesh);
instContrXml = instContrXml->NextSiblingElement("instance_controller");
}
}
/////////////////////////////////////////////////
ignition::math::Matrix4d ColladaLoader::LoadNodeTransform(TiXmlElement *_elem)
{
ignition::math::Matrix4d transform(ignition::math::Matrix4d::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("translate"))
{
std::string transStr = _elem->FirstChildElement("translate")->GetText();
ignition::math::Vector3d translate;
translate = boost::lexical_cast<ignition::math::Vector3d>(transStr);
// translate *= this->dataPtr->meter;
transform.Translate(translate);
}
TiXmlElement *rotateXml = _elem->FirstChildElement("rotate");
while (rotateXml)
{
ignition::math::Matrix3d mat;
ignition::math::Vector3d axis;
double angle;
std::string rotateStr = rotateXml->GetText();
std::istringstream iss(rotateStr);
iss >> axis.X() >> axis.Y() >> axis.Z();
iss >> angle;
mat.Axis(axis, IGN_DTOR(angle));
ignition::math::Matrix4d mat4(ignition::math::Matrix4d::Identity);
mat4 = mat;
transform = transform * mat4;
rotateXml = rotateXml->NextSiblingElement("rotate");
}
if (_elem->FirstChildElement("scale"))
{
std::string scaleStr = _elem->FirstChildElement("scale")->GetText();
ignition::math::Vector3d scale;
scale = boost::lexical_cast<ignition::math::Vector3d>(scaleStr);
ignition::math::Matrix4d scaleMat;
scaleMat.Scale(scale);
transform = transform * scaleMat;
}
}
return transform;
}
/////////////////////////////////////////////////
void ColladaLoader::LoadController(TiXmlElement *_contrXml,
TiXmlElement *_skelXml,
const ignition::math::Matrix4d &_transform, Mesh *_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");
ignition::math::Matrix4d 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;
ignition::math::Matrix4d mat;
mat.Set(ignition::math::parseFloat(strs[id + 0]),
ignition::math::parseFloat(strs[id + 1]),
ignition::math::parseFloat(strs[id + 2]),
ignition::math::parseFloat(strs[id + 3]),
ignition::math::parseFloat(strs[id + 4]),
ignition::math::parseFloat(strs[id + 5]),
ignition::math::parseFloat(strs[id + 6]),
ignition::math::parseFloat(strs[id + 7]),
ignition::math::parseFloat(strs[id + 8]),
ignition::math::parseFloat(strs[id + 9]),
ignition::math::parseFloat(strs[id + 10]),
ignition::math::parseFloat(strs[id + 11]),
ignition::math::parseFloat(strs[id + 12]),
ignition::math::parseFloat(strs[id + 13]),
ignition::math::parseFloat(strs[id + 14]),
ignition::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 ColladaLoader::LoadAnimations(TiXmlElement *_xml, Skeleton *_skel)
{
TiXmlElement *childXml = _xml->FirstChildElement("animation");
if (childXml->FirstChildElement("animation"))
{
while (childXml)
{
this->LoadAnimationSet(childXml, _skel);
childXml->NextSiblingElement("animation");
}
}
else
this->LoadAnimationSet(_xml, _skel);
}
/////////////////////////////////////////////////
void ColladaLoader::LoadAnimationSet(TiXmlElement *_xml, Skeleton *_skel)
{
std::stringstream animName;
if (_xml->Attribute("name"))
animName << _xml->Attribute("name");
else
if (_xml->Attribute("id"))
animName << _xml->Attribute("id");
else
animName << "animation" << (_skel->GetNumAnimations() + 1);
RawSkeletonAnim animation;
TiXmlElement *animXml = _xml->FirstChildElement("animation");
while (animXml)
{
TiXmlElement *chanXml = animXml->FirstChildElement("channel");
while (chanXml)
{
std::string sourceURL = chanXml->Attribute("source");
std::string targetStr = chanXml->Attribute("target");
std::string targetBone = targetStr.substr(0, targetStr.find('/'));
char sep = '0';
if (targetStr.find('(') != std::string::npos)
sep = '(';
else
if (targetStr.find('.') != std::string::npos)
sep = '.';
std::string targetTrans;
if (sep == '0')
targetTrans = targetStr.substr(targetStr.find('/') + 1);
else
targetTrans = targetStr.substr(targetStr.find('/') + 1,
targetStr.find(sep) - targetStr.find('/') - 1);
std::string idxStr = targetStr.substr(targetStr.find(sep) + 1);
int idx1 = -1;
int idx2 = -1;
if (sep == '.')
idx1 = (idxStr == "X") ? 0 : ((idxStr == "Y") ? 1 : ((idxStr == "Z")
? 2 : ((idxStr == "ANGLE") ? 3 : -1)));
else
if (sep == '(')
{
std::string idx1Str = idxStr.substr(0, 1);
idx1 = ignition::math::parseInt(idx1Str);
if (idxStr.length() > 4)
{
std::string idx2Str = idxStr.substr(3, 1);
idx2 = ignition::math::parseInt(idx2Str);
}
}
TiXmlElement *frameTimesXml = NULL;
TiXmlElement *frameTransXml = NULL;
TiXmlElement *sampXml = this->GetElementId("sampler", sourceURL);
TiXmlElement *inputXml = sampXml->FirstChildElement("input");
while (inputXml)
{
std::string semantic = inputXml->Attribute("semantic");
if (semantic == "INPUT")
frameTimesXml = this->GetElementId("source",
inputXml->Attribute("source"));
else
if (semantic == "OUTPUT")
frameTransXml = this->GetElementId("source",
inputXml->Attribute("source"));
/// FIXME interpolation semantic?
inputXml = inputXml->NextSiblingElement("input");
}
TiXmlElement *timeArray = frameTimesXml->FirstChildElement("float_array");
std::string timeStr = timeArray->GetText();
std::vector<std::string> timeStrs;
boost::split(timeStrs, timeStr, boost::is_any_of(" "));
std::vector<double> times;
for (unsigned int i = 0; i < timeStrs.size(); ++i)
times.push_back(math::parseFloat(timeStrs[i]));
TiXmlElement *output = frameTransXml->FirstChildElement("float_array");
std::string outputStr = output->GetText();
std::vector<std::string> outputStrs;
boost::split(outputStrs, outputStr, boost::is_any_of(" "));
std::vector<double> values;
for (unsigned int i = 0; i < outputStrs.size(); ++i)
values.push_back(math::parseFloat(outputStrs[i]));
TiXmlElement *accessor =
frameTransXml->FirstChildElement("technique_common");
accessor = accessor->FirstChildElement("accessor");
unsigned int stride =
ignition::math::parseInt(accessor->Attribute("stride"));
for (unsigned int i = 0; i < times.size(); ++i)
{
if (animation[targetBone].find(times[i]) == animation[targetBone].end())
animation[targetBone][times[i]] =
_skel->GetNodeById(targetBone)->GetTransforms();
std::vector<NodeTransform> *frame = &animation[targetBone][times[i]];
for (unsigned int j = 0; j < (*frame).size(); ++j)
{
NodeTransform *nt = &((*frame)[j]);
if (nt->GetSID() == targetTrans)
{
if (idx1 != -1)
{
int index = (idx2 == -1) ? idx1 : (idx1 * 4) + idx2;
nt->SetComponent(index, values[i]);
}
else
for (unsigned int k = 0; k < stride; k++)
nt->SetComponent(k, values[(i*stride) + k]);
}
}
}
chanXml = chanXml->NextSiblingElement("channel");
}
animXml = animXml->NextSiblingElement("animation");
}
SkeletonAnimation *anim = new SkeletonAnimation(animName.str());
for (RawSkeletonAnim::iterator iter = animation.begin();
iter != animation.end(); ++iter)
for (RawNodeAnim::iterator niter = iter->second.begin();
niter != iter->second.end(); ++niter)
{
ignition::math::Matrix4d transform(ignition::math::Matrix4d::Identity);
for (unsigned int i = 0; i < niter->second.size(); ++i)
{
niter->second[i].RecalculateMatrix();
transform = transform * niter->second[i]();
}
anim->AddKeyFrame(iter->first, niter->first, transform);
}
_skel->AddAnimation(anim);
}
/////////////////////////////////////////////////
SkeletonNode* ColladaLoader::LoadSkeletonNodes(TiXmlElement *_xml,
SkeletonNode *_parent)
{
std::string name;
if (_xml->Attribute("sid"))
name = _xml->Attribute("sid");
else
name = _xml->Attribute("name");
SkeletonNode* node = new SkeletonNode(_parent, name, _xml->Attribute("id"));
if (std::string(_xml->Attribute("type")) == std::string("NODE"))
node->SetType(SkeletonNode::NODE);
this->SetSkeletonNodeTransform(_xml, node);
TiXmlElement *childXml = _xml->FirstChildElement("node");
while (childXml)
{
this->LoadSkeletonNodes(childXml, node);
childXml = childXml->NextSiblingElement("node");
}
return node;
}
/////////////////////////////////////////////////
void ColladaLoader::SetSkeletonNodeTransform(TiXmlElement *_elem,
SkeletonNode *_node)
{
ignition::math::Matrix4d transform(ignition::math::Matrix4d::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]);
NodeTransform nt(transform);
nt.SetSourceValues(transform);
if (_elem->FirstChildElement("matrix")->Attribute("sid"))
nt.SetSID(_elem->FirstChildElement("matrix")->Attribute("sid"));
_node->AddRawTransform(nt);
}
else
{
if (_elem->FirstChildElement("translate"))
{
std::string transStr = _elem->FirstChildElement("translate")->GetText();
ignition::math::Vector3d translate;
translate = boost::lexical_cast<ignition::math::Vector3d>(transStr);
// translate *= this->dataPtr->meter;
transform.Translate(translate);
NodeTransform nt(transform);
if (_elem->FirstChildElement("translate")->Attribute("sid"))
nt.SetSID(_elem->FirstChildElement("translate")->Attribute("sid"));
nt.SetType(NodeTransform::TRANSLATE);
nt.SetSourceValues(translate);
_node->AddRawTransform(nt);
}
TiXmlElement *rotateXml = _elem->FirstChildElement("rotate");
while (rotateXml)
{
ignition::math::Matrix3d mat;
ignition::math::Vector3d axis;
double angle;
std::string rotateStr = rotateXml->GetText();
std::istringstream iss(rotateStr);
iss >> axis.X() >> axis.Y() >> axis.Z();
iss >> angle;
mat.Axis(axis, IGN_DTOR(angle));
ignition::math::Matrix4d mat4(ignition::math::Matrix4d::Identity);
mat4 = mat;
NodeTransform nt(mat4);
if (rotateXml->Attribute("sid"))
nt.SetSID(rotateXml->Attribute("sid"));
nt.SetType(NodeTransform::ROTATE);
nt.SetSourceValues(axis, angle);
_node->AddRawTransform(nt);
transform = transform * mat4;
rotateXml = rotateXml->NextSiblingElement("rotate");
}
if (_elem->FirstChildElement("scale"))
{
std::string scaleStr = _elem->FirstChildElement("scale")->GetText();
ignition::math::Vector3d scale;
scale = boost::lexical_cast<ignition::math::Vector3d>(scaleStr);
ignition::math::Matrix4d scaleMat;
scaleMat.Scale(scale);
NodeTransform nt(scaleMat);
if (_elem->FirstChildElement("matrix")->Attribute("sid"))
nt.SetSID(_elem->FirstChildElement("matrix")->Attribute("sid"));
nt.SetType(NodeTransform::SCALE);
nt.SetSourceValues(scale);
_node->AddRawTransform(nt);
transform = transform * scaleMat;
}
}
_node->SetTransform(transform);
}
/////////////////////////////////////////////////
void ColladaLoader::LoadGeometry(TiXmlElement *_xml,
const ignition::math::Matrix4d &_transform, Mesh *_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");
}
}
/////////////////////////////////////////////////
TiXmlElement *ColladaLoader::GetElementId(const std::string &_name,
const std::string &_id)
{
return this->GetElementId(this->dataPtr->colladaXml, _name, _id);
}
/////////////////////////////////////////////////
TiXmlElement *ColladaLoader::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 ColladaLoader::LoadVertices(const std::string &_id,
const ignition::math::Matrix4d &_transform,
std::vector<ignition::math::Vector3d> &_verts,
std::vector<ignition::math::Vector3d> &_norms)
{
std::map<unsigned int, unsigned int> vertDup;
std::map<unsigned int, unsigned int> normDup;
this->LoadVertices(_id, _transform, _verts, _norms, vertDup, normDup);
}
/////////////////////////////////////////////////
void ColladaLoader::LoadVertices(const std::string &_id,
const ignition::math::Matrix4d &_transform,
std::vector<ignition::math::Vector3d> &_verts,
std::vector<ignition::math::Vector3d> &_norms,
std::map<unsigned int, unsigned int> &_vertDups,
std::map<unsigned int, unsigned int> &_normDups)
{
TiXmlElement *verticesXml = this->GetElementId(this->dataPtr->colladaXml,
"vertices", _id);
if (!verticesXml)
{
gzerr << "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, _normDups);
}
else if (semantic == "POSITION")
{
this->LoadPositions(sourceStr, _transform, _verts, _vertDups);
}
inputXml = inputXml->NextSiblingElement("input");
}
}
/////////////////////////////////////////////////
void ColladaLoader::LoadPositions(const std::string &_id,
const ignition::math::Matrix4d &_transform,
std::vector<ignition::math::Vector3d> &_values,
std::map<unsigned int, unsigned int> &_duplicates)
{
if (this->dataPtr->positionIds.find(_id) != this->dataPtr->positionIds.end())
{
_values = this->dataPtr->positionIds[_id];
_duplicates = this->dataPtr->positionDuplicateMap[_id];
return;
}
TiXmlElement *sourceXml = this->GetElementId("source", _id);
if (!sourceXml)
{
gzerr << "Unable to find source\n";
return;
}
TiXmlElement *floatArrayXml = sourceXml->FirstChildElement("float_array");
if (!floatArrayXml || !floatArrayXml->GetText())
{
int count = 1;
if (floatArrayXml && floatArrayXml->Attribute("count"))
{
try
{
count = boost::lexical_cast<int>(floatArrayXml->Attribute("count"));
}
catch(...)
{
// Do nothing. Messages are printed out below.
}
}
if (count)
{
gzerr << "Vertex source missing float_array element, "
<< "or count is invalid.\n";
}
else
{
gzlog << "Vertex source has a float_array with a count of zero. "
<< "This is likely not desired\n";
}
return;
}
std::string valueStr = floatArrayXml->GetText();
boost::unordered_map<ignition::math::Vector3d,
unsigned int, Vector3Hash> unique;
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)
{
ignition::math::Vector3d vec(math::parseFloat(*iter),
ignition::math::parseFloat(*(iter+1)),
ignition::math::parseFloat(*(iter+2)));
vec = _transform * vec;
_values.push_back(vec);
// create a map of duplicate indices
if (unique.find(vec) != unique.end())
_duplicates[_values.size()-1] = unique[vec];
else
unique[vec] = _values.size()-1;
}
this->dataPtr->positionDuplicateMap[_id] = _duplicates;
this->dataPtr->positionIds[_id] = _values;
}
/////////////////////////////////////////////////
void ColladaLoader::LoadNormals(const std::string &_id,
const ignition::math::Matrix4d &_transform,
std::vector<ignition::math::Vector3d> &_values,
std::map<unsigned int, unsigned int> &_duplicates)
{
if (this->dataPtr->normalIds.find(_id) != this->dataPtr->normalIds.end())
{
_values = this->dataPtr->normalIds[_id];
_duplicates = this->dataPtr->normalDuplicateMap[_id];
return;
}
ignition::math::Matrix4d rotMat = _transform;
rotMat.Translate(ignition::math::Vector3d::Zero);
TiXmlElement *normalsXml = this->GetElementId("source", _id);
if (!normalsXml)
{
gzerr << "Unable to find normals[" << _id << "] in collada file\n";
return;
}
TiXmlElement *floatArrayXml = normalsXml->FirstChildElement("float_array");
if (!floatArrayXml || !floatArrayXml->GetText())
{
int count = 1;
if (floatArrayXml && floatArrayXml->Attribute("count"))
{
try
{
count = boost::lexical_cast<int>(floatArrayXml->Attribute("count"));
}
catch(...)
{
// Do nothing. Messages are printed out below.
}
}
if (count)
{
gzwarn << "Normal source missing float_array element, or count is "
<< "invalid.\n";
}
else
{
gzlog << "Normal source has a float_array with a count of zero. "
<< "This is likely not desired\n";
}
return;
}
boost::unordered_map<ignition::math::Vector3d,
unsigned int, Vector3Hash> unique;
std::string valueStr = floatArrayXml->GetText();
std::istringstream iss(valueStr);
do
{
ignition::math::Vector3d vec;
iss >> vec.X() >> vec.Y() >> vec.Z();
if (iss)
{
vec = rotMat * vec;
vec.Normalize();
_values.push_back(vec);
// create a map of duplicate indices
if (unique.find(vec) != unique.end())
_duplicates[_values.size()-1] = unique[vec];
else
unique[vec] = _values.size()-1;
}
} while (iss);
this->dataPtr->normalDuplicateMap[_id] = _duplicates;
this->dataPtr->normalIds[_id] = _values;
}
/////////////////////////////////////////////////
void ColladaLoader::LoadTexCoords(const std::string &_id,
std::vector<ignition::math::Vector2d> &_values,
std::map<unsigned int, unsigned int> &_duplicates)
{
if (this->dataPtr->texcoordIds.find(_id) != this->dataPtr->texcoordIds.end())
{
_values = this->dataPtr->texcoordIds[_id];
_duplicates = this->dataPtr->texcoordDuplicateMap[_id];
return;
}
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)
{
gzerr << "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 || !floatArrayXml->GetText())
{
int count = 1;
if (floatArrayXml && floatArrayXml->Attribute("count"))
{
try
{
count = boost::lexical_cast<int>(floatArrayXml->Attribute("count"));
}
catch(...)
{
// Do nothing. Messages are printed out below.
}
}
if (count)
{
gzerr << "Normal source missing float_array element, or count is "
<< "invalid.\n";
}
else
{
gzlog << "Normal source has a float_array with a count of zero. "
<< "This is likely not desired\n";
}
return;
}
// Read in the total number of texture coordinate values
else if (floatArrayXml->Attribute("count"))
totCount = boost::lexical_cast<int>(floatArrayXml->Attribute("count"));
else
{
gzerr << "<float_array> has no count attribute in texture coordinate "
<< "element with id[" << _id << "]\n";
return;
}
// The technique_common holds an <accessor> element that indicates how to
// parse the float array.
xml = xml->FirstChildElement("technique_common");
if (!xml)
{
gzerr << "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)
{
gzerr << "Unable to find <accessor> as a child of <technique_common> "
<< "for texture coordinates 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
{
gzerr << "<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
{
gzerr << "<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)
{
gzerr << "Error reading texture coordinates. Coordinate counts in element "
"with id[" << _id << "] do not add up correctly\n";
return;
}
// Nothing to read. Don't print a warning because the collada file is
// correct.
if (totCount == 0)
return;
boost::unordered_map<ignition::math::Vector2d,
unsigned int, Vector2dHash> unique;
// 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.
ignition::math::Vector2d vec(boost::lexical_cast<double>(values[i]),
1.0 - boost::lexical_cast<double>(values[i+1]));
_values.push_back(vec);
// create a map of duplicate indices
if (unique.find(vec) != unique.end())
{
_duplicates[_values.size()-1] = unique[vec];
}
else
unique[vec] = _values.size()-1;
}
this->dataPtr->texcoordDuplicateMap[_id] = _duplicates;
this->dataPtr->texcoordIds[_id] = _values;
}
/////////////////////////////////////////////////
Material *ColladaLoader::LoadMaterial(const std::string &_name)
{
if (this->dataPtr->materialIds.find(_name)
!= this->dataPtr->materialIds.end())
{
return this->dataPtr->materialIds[_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)
gzerr << "profile_GLSL unsupported\n";
TiXmlElement *cgXml = effectXml->FirstChildElement("profile_CG");
if (cgXml)
gzerr << "profile_CG unsupported\n";
this->dataPtr->materialIds[_name] = mat;
return mat;
}
/////////////////////////////////////////////////
void ColladaLoader::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 (_type == "specular")
_mat->SetSpecular(color);
}
else if (typeElem->FirstChildElement("texture"))
{
_mat->SetLighting(true);
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->dataPtr->path);
}
}
}
/////////////////////////////////////////////////
void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
const ignition::math::Matrix4d &_transform,
Mesh *_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
SubMesh *subMesh = new SubMesh;
subMesh->SetName(this->dataPtr->currentNodeName);
bool combinedVertNorms = false;
subMesh->SetPrimitiveType(SubMesh::TRIANGLES);
if (_polylistXml->Attribute("material"))
{
std::map<std::string, std::string>::iterator iter;
std::string matStr = _polylistXml->Attribute("material");
int matIndex = -1;
iter = this->dataPtr->materialMap.find(matStr);
if (iter != this->dataPtr->materialMap.end())
matStr = iter->second;
common::Material *mat = this->LoadMaterial(matStr);
matIndex = _mesh->GetMaterialIndex(mat);
if (matIndex < 0)
matIndex = _mesh->AddMaterial(mat);
if (matIndex < 0)
gzwarn << "Unable to add material[" << matStr << "]\n";
else
subMesh->SetMaterialIndex(matIndex);
}
TiXmlElement *polylistInputXml = _polylistXml->FirstChildElement("input");
std::vector<ignition::math::Vector3d> verts;
std::vector<ignition::math::Vector3d> norms;
std::vector<ignition::math::Vector2d> texcoords;
const unsigned int VERTEX = 0;
const unsigned int NORMAL = 1;
const unsigned int TEXCOORD = 2;
unsigned int otherSemantics = TEXCOORD + 1;
// look up table of position/normal/texcoord duplicate indices
std::map<unsigned int, unsigned int> texDupMap;
std::map<unsigned int, unsigned int> normalDupMap;
std::map<unsigned int, unsigned int> positionDupMap;
ignition::math::Matrix4d bindShapeMat(ignition::math::Matrix4d::Identity);
if (_mesh->HasSkeleton())
bindShapeMat = _mesh->GetSkeleton()->BindShapeTransform();
// read input elements. A vector of int is used because there can be
// multiple TEXCOORD inputs.
std::map<const unsigned int, std::set<int>> inputs;
unsigned int inputSize = 0;
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,
positionDupMap, normalDupMap);
if (norms.size() > count)
combinedVertNorms = true;
inputs[VERTEX].insert(ignition::math::parseInt(offset));
}
else if (semantic == "NORMAL")
{
this->LoadNormals(source, _transform, norms, normalDupMap);
combinedVertNorms = false;
inputs[NORMAL].insert(ignition::math::parseInt(offset));
}
else if (semantic == "TEXCOORD")
{
this->LoadTexCoords(source, texcoords, texDupMap);
inputs[TEXCOORD].insert(ignition::math::parseInt(offset));
}
else
{
inputs[otherSemantics++].insert(ignition::math::parseInt(offset));
gzwarn << "Polylist input semantic: '" << semantic << "' is currently"
<< " not supported" << std::endl;
}
polylistInputXml = polylistInputXml->NextSiblingElement("input");
}
for (const auto &input : inputs)
inputSize += input.second.size();
// 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();
// vertexIndexMap is a map of collada vertex index to Gazebo submesh vertex
// indices, used for identifying vertices that can be shared.
std::map<unsigned int, std::vector<GeometryIndices> > vertexIndexMap;
unsigned int *values = new unsigned int[inputSize];
memset(values, 0, inputSize);
std::vector<std::string> strs;
boost::split(strs, pStr, boost::is_any_of(" "));
std::vector<std::string>::iterator strsIter = strs.begin();
for (unsigned int l = 0; l < vcounts.size(); ++l)
{
// put us at the beginning of the polygon list
if (l > 0)
strsIter += inputSize*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)*inputSize;
if (j == 2)
triangle_index = (k)*inputSize;
for (unsigned int i = 0; i < inputSize; ++i)
{
values[i] = ignition::math::parseInt(strsIter[triangle_index+i]);
/*gzerr << "debug parsing "
<< " poly-i[" << l
<< "] tri-end-index[" << k
<< "] tri-vertex-i[" << j
<< "] triangle[" << triangle_index
<< "] input[" << i
<< "] value[" << values[i]
<< "]\n"; */
}
unsigned int daeVertIndex = 0;
bool addIndex = inputs[VERTEX].empty();
// find a set of vertex/normal/texcoord that can be reused
// only do this if the mesh has vertices
if (!inputs[VERTEX].empty())
{
// Get the vertex position index value. If it is a duplicate then use
// the existing index instead
daeVertIndex = values[*inputs[VERTEX].begin()];
if (positionDupMap.find(daeVertIndex) != positionDupMap.end())
daeVertIndex = positionDupMap[daeVertIndex];
// if the vertex index has not been previously added then just add it.
if (vertexIndexMap.find(daeVertIndex) == vertexIndexMap.end())
{
addIndex = true;
}
else
{
// if the vertex index was previously added, check to see if it has
// the same normal and texcoord index values
bool toDuplicate = true;
unsigned int reuseIndex = 0;
std::vector<GeometryIndices> inputValues =
vertexIndexMap[daeVertIndex];
for (unsigned int i = 0; i < inputValues.size(); ++i)
{
GeometryIndices iv = inputValues[i];
bool normEqual = false;
bool texEqual = false;
if (!inputs[NORMAL].empty())
{
// Get the vertex normal index value. If the normal is a
// duplicate then reset the index to the first instance of the
// duplicated position
unsigned int remappedNormalIndex =
values[*inputs[NORMAL].begin()];
if (normalDupMap.find(remappedNormalIndex)
!= normalDupMap.end())
{
remappedNormalIndex = normalDupMap[remappedNormalIndex];
}
if (iv.normalIndex == remappedNormalIndex)
normEqual = true;
}
if (!inputs[TEXCOORD].empty())
{
// \todo: Add support for multiple texture maps to SubMesh.
// Here we are only using the first texture coordinates, when
// multiple could have been specified. See Gazebo issue #532.
// Get the vertex texcoord index value. If the texcoord is a
// duplicate then reset the index to the first instance of the
// duplicated texcoord
unsigned int remappedTexcoordIndex =
values[*inputs[TEXCOORD].begin()];
if (texDupMap.find(remappedTexcoordIndex) != texDupMap.end())
remappedTexcoordIndex = texDupMap[remappedTexcoordIndex];
texEqual = iv.texcoordIndex == remappedTexcoordIndex;
}
// if the vertex has matching normal and texcoord index values
// then the vertex can be reused.
if ((inputs[NORMAL].empty() || normEqual) &&
(inputs[TEXCOORD].empty() || texEqual))
{
// found a vertex that can be shared.
toDuplicate = false;
reuseIndex = iv.mappedIndex;
subMesh->AddIndex(reuseIndex);
break;
}
}
addIndex = toDuplicate;
}
}
// if the vertex index is new or can not be shared then add it
if (addIndex)
{
GeometryIndices input;
if (!inputs[VERTEX].empty())
{
subMesh->AddVertex(verts[daeVertIndex]);
unsigned int newVertIndex = subMesh->GetVertexCount()-1;
subMesh->AddIndex(newVertIndex);
if (combinedVertNorms)
subMesh->AddNormal(norms[daeVertIndex]);
if (_mesh->HasSkeleton())
{
subMesh->SetVertex(newVertIndex, bindShapeMat *
subMesh->Vertex(newVertIndex));
Skeleton *skel = _mesh->GetSkeleton();
for (unsigned int i = 0;
i < skel->GetNumVertNodeWeights(daeVertIndex); ++i)
{
std::pair<std::string, double> node_weight =
skel->GetVertNodeWeight(daeVertIndex, i);
SkeletonNode *node =
_mesh->GetSkeleton()->GetNodeByName(node_weight.first);
subMesh->AddNodeAssignment(subMesh->GetVertexCount()-1,
node->GetHandle(), node_weight.second);
}
}
input.vertexIndex = daeVertIndex;
input.mappedIndex = newVertIndex;
}
if (!inputs[NORMAL].empty())
{
unsigned int inputRemappedNormalIndex =
values[*inputs[NORMAL].begin()];
if (normalDupMap.find(inputRemappedNormalIndex)
!= normalDupMap.end())
inputRemappedNormalIndex = normalDupMap[inputRemappedNormalIndex];
subMesh->AddNormal(norms[inputRemappedNormalIndex]);
input.normalIndex = inputRemappedNormalIndex;
}
if (!inputs[TEXCOORD].empty())
{
// \todo: Add support for multiple texture maps to SubMesh.
// Here we are only using the first texture coordinates, when
// multiple could have been specified.
unsigned int inputRemappedTexcoordIndex =
values[*inputs[TEXCOORD].begin()];
if (texDupMap.find(inputRemappedTexcoordIndex) != texDupMap.end())
{
inputRemappedTexcoordIndex =
texDupMap[inputRemappedTexcoordIndex];
}
subMesh->AddTexCoord(texcoords[inputRemappedTexcoordIndex].X(),
texcoords[inputRemappedTexcoordIndex].Y());
input.texcoordIndex = inputRemappedTexcoordIndex;
}
// add the new gazebo submesh vertex index to the map
if (!inputs[VERTEX].empty())
{
std::vector<GeometryIndices> inputValues;
inputValues.push_back(input);
vertexIndexMap[daeVertIndex] = inputValues;
}
}
}
}
}
delete [] values;
_mesh->AddSubMesh(subMesh);
}
/////////////////////////////////////////////////
void ColladaLoader::LoadTriangles(TiXmlElement *_trianglesXml,
const ignition::math::Matrix4d &_transform,
Mesh *_mesh)
{
SubMesh *subMesh = new SubMesh;
subMesh->SetName(this->dataPtr->currentNodeName);
bool combinedVertNorms = false;
subMesh->SetPrimitiveType(SubMesh::TRIANGLES);
if (_trianglesXml->Attribute("material"))
{
std::map<std::string, std::string>::iterator iter;
std::string matStr = _trianglesXml->Attribute("material");
int matIndex = -1;
iter = this->dataPtr->materialMap.find(matStr);
if (iter != this->dataPtr->materialMap.end())
matStr = iter->second;
common::Material *mat = this->LoadMaterial(matStr);
matIndex = _mesh->GetMaterialIndex(mat);
if (matIndex < 0)
matIndex = _mesh->AddMaterial(mat);
if (matIndex < 0)
gzwarn << "Unable to add material[" << matStr << "]\n";
else
subMesh->SetMaterialIndex(matIndex);
}
TiXmlElement *trianglesInputXml = _trianglesXml->FirstChildElement("input");
std::vector<ignition::math::Vector3d> verts;
std::vector<ignition::math::Vector3d> norms;
std::vector<ignition::math::Vector2d> texcoords;
const unsigned int VERTEX = 0;
const unsigned int NORMAL = 1;
const unsigned int TEXCOORD = 2;
unsigned int otherSemantics = TEXCOORD + 1;
bool hasVertices = false;
bool hasNormals = false;
bool hasTexcoords = false;
unsigned int offsetSize = 0;
// read input elements. A vector of int is used because there can be
// multiple TEXCOORD inputs.
std::map<const unsigned int, std::set<int>> inputs;
// look up table of position/normal/texcoord duplicate indices
std::map<unsigned int, unsigned int> texDupMap;
std::map<unsigned int, unsigned int> normalDupMap;
std::map<unsigned int, unsigned int> positionDupMap;
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,
positionDupMap, normalDupMap);
if (norms.size() > count)
combinedVertNorms = true;
inputs[VERTEX].insert(ignition::math::parseInt(offset));
hasVertices = true;
}
else if (semantic == "NORMAL")
{
this->LoadNormals(source, _transform, norms, normalDupMap);
combinedVertNorms = false;
inputs[NORMAL].insert(ignition::math::parseInt(offset));
hasNormals = true;
}
else if (semantic == "TEXCOORD")
{
// we currently only support one set of UVs
this->LoadTexCoords(source, texcoords, texDupMap);
inputs[TEXCOORD].insert(ignition::math::parseInt(offset));
hasTexcoords = true;
}
else
{
inputs[otherSemantics++].insert(ignition::math::parseInt(offset));
gzwarn << "Triangle input semantic: '" << semantic << "' is currently"
<< " not supported" << std::endl;
}
trianglesInputXml = trianglesInputXml->NextSiblingElement("input");
}
for (const auto &input : inputs)
offsetSize += input.second.size();
TiXmlElement *pXml = _trianglesXml->FirstChildElement("p");
if (!pXml || !pXml->GetText())
{
int count = 1;
if (_trianglesXml->Attribute("count"))
{
try
{
count = boost::lexical_cast<int>(_trianglesXml->Attribute("count"));
}
catch(...)
{
// Do nothing. Messages are printed out below.
}
}
// It's possible that the triangle count is zero. In this case, we
// should not output an error message
if (count)
{
gzerr << "Collada file[" << this->dataPtr->filename
<< "] is invalid. Loading what we can...\n";
}
else
{
gzlog << "Triangle input has a count of zero. "
<< "This is likely not desired\n";
}
return;
}
std::string pStr = pXml->GetText();
// Collada format allows normals and texcoords to have their own set of
// indices for more efficient storage of data but opengl only supports one
// index buffer. So we need to reorder normals/texcoord to match the vertex
// index and duplicate any vertices that have the same index but different
// normal/texcoord.
// vertexIndexMap is a map of collada vertex index to Gazebo submesh vertex
// indices, used for identifying vertices that can be shared.
std::map<unsigned int, std::vector<GeometryIndices> > vertexIndexMap;
unsigned int *values = new unsigned int[offsetSize];
std::vector<std::string> strs;
boost::split(strs, pStr, boost::is_any_of(" "));
for (unsigned int j = 0; j < strs.size(); j += offsetSize)
{
for (unsigned int i = 0; i < offsetSize; ++i)
values[i] = ignition::math::parseInt(strs[j+i]);
unsigned int daeVertIndex = 0;
bool addIndex = !hasVertices;
// find a set of vertex/normal/texcoord that can be reused
// only do this if the mesh has vertices
if (hasVertices)
{
// Get the vertex position index value. If the position is a duplicate
// then reset the index to the first instance of the duplicated position
daeVertIndex = values[*inputs[VERTEX].begin()];
if (positionDupMap.find(daeVertIndex) != positionDupMap.end())
daeVertIndex = positionDupMap[daeVertIndex];
// if the vertex index has not been previously added then just add it.
if (vertexIndexMap.find(daeVertIndex) == vertexIndexMap.end())
{
addIndex = true;
}
else
{
// if the vertex index was previously added, check to see if it has the
// same normal and texcoord index values
bool toDuplicate = true;
unsigned int reuseIndex = 0;
std::vector<GeometryIndices> inputValues = vertexIndexMap[daeVertIndex];
for (unsigned int i = 0; i < inputValues.size(); ++i)
{
GeometryIndices iv = inputValues[i];
bool normEqual = false;
bool texEqual = false;
if (hasNormals)
{
// Get the vertex normal index value. If the normal is a duplicate
// then reset the index to the first instance of the duplicated
// position
unsigned int remappedNormalIndex = values[*inputs[NORMAL].begin()];
if (normalDupMap.find(remappedNormalIndex) != normalDupMap.end())
remappedNormalIndex = normalDupMap[remappedNormalIndex];
if (iv.normalIndex == remappedNormalIndex)
normEqual = true;
}
if (hasTexcoords)
{
// Get the vertex texcoord index value. If the texcoord is a
// duplicate then reset the index to the first instance of the
// duplicated texcoord
unsigned int remappedTexcoordIndex =
values[*inputs[TEXCOORD].begin()];
if (texDupMap.find(remappedTexcoordIndex) != texDupMap.end())
remappedTexcoordIndex = texDupMap[remappedTexcoordIndex];
if (iv.texcoordIndex == remappedTexcoordIndex)
texEqual = true;
}
// if the vertex has matching normal and texcoord index values then
// the vertex can be reused.
if ((!hasNormals || normEqual) && (!hasTexcoords || texEqual))
{
// found a vertex that can be shared.
toDuplicate = false;
reuseIndex = iv.mappedIndex;
subMesh->AddIndex(reuseIndex);
break;
}
}
addIndex = toDuplicate;
}
}
// if the vertex index is new or can not be shared then add it
if (addIndex)
{
GeometryIndices input;
if (hasVertices)
{
subMesh->AddVertex(verts[daeVertIndex]);
unsigned int newVertIndex = subMesh->GetVertexCount()-1;
subMesh->AddIndex(newVertIndex);
if (combinedVertNorms)
subMesh->AddNormal(norms[daeVertIndex]);
if (_mesh->HasSkeleton())
{
Skeleton *skel = _mesh->GetSkeleton();
for (unsigned int i = 0;
i < skel->GetNumVertNodeWeights(values[daeVertIndex]); ++i)
{
std::pair<std::string, double> node_weight =
skel->GetVertNodeWeight(values[daeVertIndex], i);
SkeletonNode *node =
_mesh->GetSkeleton()->GetNodeByName(node_weight.first);
subMesh->AddNodeAssignment(subMesh->GetVertexCount()-1,
node->GetHandle(), node_weight.second);
}
}
input.vertexIndex = daeVertIndex;
input.mappedIndex = newVertIndex;
}
if (hasNormals)
{
unsigned int inputRemappedNormalIndex = values[*inputs[NORMAL].begin()];
if (normalDupMap.find(inputRemappedNormalIndex) != normalDupMap.end())
inputRemappedNormalIndex = normalDupMap[inputRemappedNormalIndex];
subMesh->AddNormal(norms[inputRemappedNormalIndex]);
input.normalIndex = inputRemappedNormalIndex;
}
if (hasTexcoords)
{
unsigned int inputRemappedTexcoordIndex =
values[*inputs[TEXCOORD].begin()];
if (texDupMap.find(inputRemappedTexcoordIndex) != texDupMap.end())
inputRemappedTexcoordIndex = texDupMap[inputRemappedTexcoordIndex];
subMesh->AddTexCoord(texcoords[inputRemappedTexcoordIndex].X(),
texcoords[inputRemappedTexcoordIndex].Y());
input.texcoordIndex = inputRemappedTexcoordIndex;
}
// add the new gazebo submesh vertex index to the map
if (hasVertices)
{
std::vector<GeometryIndices> inputValues;
inputValues.push_back(input);
vertexIndexMap[daeVertIndex] = inputValues;
}
}
}
delete [] values;
_mesh->AddSubMesh(subMesh);
}
/////////////////////////////////////////////////
void ColladaLoader::LoadLines(TiXmlElement *_xml,
const ignition::math::Matrix4d &_transform,
Mesh *_mesh)
{
SubMesh *subMesh = new SubMesh;
subMesh->SetName(this->dataPtr->currentNodeName);
subMesh->SetPrimitiveType(SubMesh::LINES);
TiXmlElement *inputXml = _xml->FirstChildElement("input");
// std::string semantic = inputXml->Attribute("semantic");
std::string source = inputXml->Attribute("source");
std::vector<ignition::math::Vector3d> verts;
std::vector<ignition::math::Vector3d> 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;
subMesh->AddVertex(verts[a]);
subMesh->AddIndex(subMesh->GetVertexCount() - 1);
subMesh->AddVertex(verts[b]);
subMesh->AddIndex(subMesh->GetVertexCount() - 1);
} while (iss);
_mesh->AddSubMesh(subMesh);
}
/////////////////////////////////////////////////
float ColladaLoader::LoadFloat(TiXmlElement *_elem)
{
float value = 0;
if (_elem->FirstChildElement("float"))
{
value =
ignition::math::parseFloat(_elem->FirstChildElement("float")->GetText());
}
return value;
}
/////////////////////////////////////////////////
void ColladaLoader::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)
{
gzerr << "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);
}
}
Reference in New Issue View Git Blame Copy Permalink