forked from bartvdbraak/blender
Fix iTaSC build error when building with libc++.
This was using TreeElement before it was fully defined, which gives undefined behavior that happened to work with other libraries but not libc++. Based on patch by Marcus von Appen, modifications for brevity and to ensure we don't dereference invalid memory. Ref T37477.
This commit is contained in:
parent
28a2d5e264
commit
19a41e5a10
@ -402,7 +402,7 @@ bool Armature::finalize()
|
||||
m_armlength = 0.0;
|
||||
for (i=0; i<m_neffector; i++) {
|
||||
length = 0.0;
|
||||
KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name);
|
||||
KDL::SegmentMap::value_type const *sit = m_tree.getSegmentPtr(m_effectors[i].name);
|
||||
while (sit->first != "root") {
|
||||
Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
|
||||
length += tip.p.Norm();
|
||||
|
@ -64,7 +64,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
|
||||
return false;
|
||||
pair<SegmentMap::iterator, bool> retval;
|
||||
//insert new element
|
||||
TreeElement elem(segment, parent, nrOfJoints);
|
||||
TreeElement elem(segment, *parent, nrOfJoints);
|
||||
std::pair<std::string, TreeElement> val(segment_name, elem);
|
||||
|
||||
retval = segments.insert(val);
|
||||
|
@ -41,32 +41,28 @@ namespace KDL
|
||||
{
|
||||
//Forward declaration
|
||||
class TreeElement;
|
||||
#if defined(__APPLE__)
|
||||
# if MAC_OS_X_VERSION_MIN_REQUIRED <= 1050
|
||||
#if defined(__APPLE__) && MAC_OS_X_VERSION_MIN_REQUIRED <= 1050
|
||||
typedef std::map<std::string,TreeElement> SegmentMap;
|
||||
# else
|
||||
// Eigen allocator is needed for alignment of Eigen data types
|
||||
typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<std::string, TreeElement> > > SegmentMap;
|
||||
# endif /* MAC_OS_X_VERSION_MIN_REQUIRED */
|
||||
#else
|
||||
// Eigen allocator is needed for alignment of Eigen data types
|
||||
typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<std::string, TreeElement> > > SegmentMap;
|
||||
#endif
|
||||
|
||||
class TreeElement
|
||||
{
|
||||
public:
|
||||
TreeElement():q_nr(0)
|
||||
TreeElement():q_nr(0),parent(0)
|
||||
{};
|
||||
public:
|
||||
Segment segment;
|
||||
unsigned int q_nr;
|
||||
SegmentMap::const_iterator parent;
|
||||
SegmentMap::value_type const *parent;
|
||||
std::vector<SegmentMap::const_iterator > children;
|
||||
TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in)
|
||||
TreeElement(const Segment& segment_in,const SegmentMap::value_type& parent_in,unsigned int q_nr_in)
|
||||
{
|
||||
q_nr=q_nr_in;
|
||||
segment=segment_in;
|
||||
parent=parent_in;
|
||||
parent=&parent_in;
|
||||
};
|
||||
static TreeElement Root()
|
||||
{
|
||||
@ -167,7 +163,15 @@ namespace KDL
|
||||
return segments.find(segment_name);
|
||||
};
|
||||
|
||||
SegmentMap::value_type const* getSegmentPtr(const std::string& segment_name)const
|
||||
{
|
||||
SegmentMap::const_iterator it = segments.find(segment_name);
|
||||
|
||||
if (it == segments.end())
|
||||
return 0;
|
||||
|
||||
return &*it;
|
||||
};
|
||||
|
||||
const SegmentMap& getSegments()const
|
||||
{
|
||||
|
@ -35,22 +35,22 @@ namespace KDL {
|
||||
|
||||
int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
|
||||
{
|
||||
SegmentMap::const_iterator it = tree.getSegment(segmentName);
|
||||
SegmentMap::const_iterator baseit = tree.getSegment(baseName);
|
||||
SegmentMap::value_type const* it = tree.getSegmentPtr(segmentName);
|
||||
SegmentMap::value_type const* baseit = tree.getSegmentPtr(baseName);
|
||||
|
||||
if(q_in.rows() != tree.getNrOfJoints())
|
||||
return -1;
|
||||
else if(it == tree.getSegments().end()) //if the segment name is not found
|
||||
else if(!it) //if the segment name is not found
|
||||
return -2;
|
||||
else if(baseit == tree.getSegments().end()) //if the base segment name is not found
|
||||
else if(!baseit) //if the base segment name is not found
|
||||
return -3;
|
||||
else{
|
||||
p_out = recursiveFk(q_in, it, baseit);
|
||||
return 0;
|
||||
p_out = recursiveFk(q_in, it, baseit);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit)
|
||||
Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit)
|
||||
{
|
||||
//gets the frame for the current element (segment)
|
||||
const TreeElement& currentElement = it->second;
|
||||
@ -60,8 +60,7 @@ namespace KDL {
|
||||
}
|
||||
else{
|
||||
Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
|
||||
SegmentMap::const_iterator parentIt = currentElement.parent;
|
||||
return recursiveFk(q_in, parentIt, baseit) * currentFrame;
|
||||
return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -45,7 +45,7 @@ namespace KDL {
|
||||
private:
|
||||
const Tree tree;
|
||||
|
||||
Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit);
|
||||
Frame recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit);
|
||||
};
|
||||
|
||||
}
|
||||
|
@ -28,16 +28,16 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
|
||||
return -1;
|
||||
|
||||
//Lets search the tree-element
|
||||
SegmentMap::const_iterator it = tree.getSegments().find(segmentname);
|
||||
SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);
|
||||
|
||||
//If segmentname is not inside the tree, back out:
|
||||
if (it == tree.getSegments().end())
|
||||
if (!it)
|
||||
return -2;
|
||||
|
||||
//Let's make the jacobian zero:
|
||||
SetToZero(jac);
|
||||
|
||||
SegmentMap::const_iterator root = tree.getSegments().find("root");
|
||||
SegmentMap::value_type const* root = tree.getSegmentPtr("root");
|
||||
|
||||
Frame T_total = Frame::Identity();
|
||||
Frame T_local, T_joint;
|
||||
|
Loading…
Reference in New Issue
Block a user